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Abstract 


An  iterative  method  is  developed  by  which  the  contact  forces  required  to  apply 
an  arbitrary  wrench  (six  elements  of  force  and  moment)  to  a  stably  grasped  object 
may  be  calculated  quickly.  The  assignment  of  contact  forces,  given  a  required  object 
wrench,  is  accomplished  with  the  use  of  fuzzy  logic.  This  concept  is  referred  to  as 
the  fuzzy  logic  reactive  system  (FLRS).  The  solution  is  versatile  with  respect  to 
goals  inherent  in  the  rulebase  and  the  input  parameters,  and  is  also  applicable  for 
an  arbitrary  number  of  contacts.  The  goal  presented  in  this  research,  to  illustrate 
the  concept  of  the  FLRS,  is  the  minimization  of  the  norm  of  the  contact  forces  using 
point  contacts  with  friction.  Results  comparing  the  contact  force  assignment  for 
this  method  and  the  optimal  method  proposed  by  Nakamura  are  presented.  The 
results  show  that  FLRS  will  satisfy  the  object  wrench  and  frictional  contacts  while 
achieving  near  optimal  contact  force  assignment.  This  method  is  shown  to  require 
significantly  fewer  floating  point  operations  than  the  solution  calculated  using  nu¬ 
merical  constrained  optimization  techniques. 
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Reaction  Based  Grasp  Force  Assignment 


1.  Introduction 

1.1  Motivation 

The  word  robot  was  introduced  to  the  world  through  a  satirical  drama  writ¬ 
ten  in  1921  by  the  Czech  playwright  Karl  Capek  [26].  The  robots  were  depicted  as 
anthropomorphic  machines  which  excelled  in  the  physically  demanding  work  envi¬ 
ronment  of  the  early  twentieth  century.  This  idealistic  vision  of  machines,  equipped 
with  human  looking  dextrous  hands,  has  yet  to  be  realized.  This  is  due  in  part  to 
the  complex  nature  of  object  manipulation  by  multifingered  dextrous  hands. 

Object  manipulation  is  a  common  problem  for  robots,  prosthetics,  and  artifi¬ 
cially  stimulated  biological  hands  [14,  40,  36,  33,  23].  The  recent  past  is  filled  with 
various  control  architectures,  grasp  philosophies,  and  numerical  methods  which  have 
yet  to  equal  the  human  ability  to  stably  manipulate  grasped  objects  [4,  37,  43]. 
Grasped  object  manipulation  has  been,  and  continues  to  be,  an  active  area  of  re¬ 
search  with  many  papers  produced  for  robotics  journals,  conferences,  and  sympo¬ 
siums  [60,  25,  9].  The  goal  of  this  research  is  to  contribute  to  the  body  of  knowledge 
of  real-time  solution  techniques,  concerning  the  problem  of  stable  manipulation  of 
grasped  objects. 

Many  robots  today  are  limited  to  environments  where  the  robot  tools  are 
essentially  special  end-effectors  locked  in  place  which  may  or  may  not  be  difficult 
to  change.  In  any  case,  the  tools  are  of  limited  use  and  of  special  design  consistent 
with  structured  use  of  the  robot.  This  lack  of  tool  generality  limits  the  ability  of  a 
robot  to  function  in  unstructured  environments.  This  in  turn  limits  the  argument  for 
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robots  in  general,  since  hard  automation  machines  may  be  more  suited  for  exclusively 
repetitive  tasks  in  a  predictable  environment. 

The  manipulation  of  an  object  by  humans  is  greatly  enhanced  by  the  usually 
seamless  integration  of  sensors,  actuators,  and  controllers  acting  on  multiple  levels. 
The  technological  challenge  facing  engineers  and  scientists  today,  related  to  grasping 
and  manipulation,  is  the  development  of  integrated  artificial  ’’hands”  as  capable  as 
those  of  humans.  Such  integration  involves  low  level  manipulative  elements  as  well 
as  higher  level  planning  elements.  The  lower  level  elements  are  responsible  for  the 
mechanics  of  basic  object  manipulation.  One  of  the  elements  necessary  to  successfully 
control  a  redundantly  grasped  object  is  a  fast  contact  force  assignment  method  based 
on  commanded  object  behavior.  Many  current  methods  emphasize  solutions  which 
are  of  limited  application  or  numerically  intensive  and  globally  optimal.  This  work 
investigates  a  different  approach  to  the  solution  of  this  problem. 

1.2  Overview  of  Conventional  Grasped  Object  Manipulation  Methods 

The  literature  defines  several  classes  of  robotic  grasps.  Napier  suggests  that 
human  grasps  are  variations  of  either  a  power  or  precision  grasp,  we  will  examine 
the  precision  grasp  only  [41].  The  precision  grasp  is  one  in  which  the  grasped  object 
is  contacted  by  the  fingertips  only  and  is  generally  associated  with  fine  manipulation 
under  low  force  applications.  Different  forms  of  control  architectures  for  the  manip¬ 
ulation  of  grasped  objects  have  been  proposed.  Nakamura,  among  others,  separates 
the  grasped  object  control  problem  into  two  parts,  dynamic  and  static.  The  dy¬ 
namic  portion  concerns  the  determination  of  joint  torques  required  to  accelerate  the 
end-effectors  of  the  finger  manipulators  to  follow  the  motion  of  the  contact  points 
on  the  accelerated  object.  The  static  portion  controls  the  joint  torques  required  to 
satisfy  contact  stability  and  accelerate  the  object  [39].  Schneider  and  Cannon  use 
this  architecture,  but  also  include  external  forces  applied  to  the  object  among  static 
force  components  [49].  Schneider  and  Cannon  describe  a  feedback  system  which  is 
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an  implementation  of  object  based  impedance  control  based  on  the  work  of  Hogan 
[20].  Their  control  scheme  enforces  a  controlled  impedance  of  the  object  and  thus 
environmental  contacts  are  easily  handled. 

Li,  Hsu,  and  Sastry  have  proposed  a  unified  control  scheme  which  seeks  to 
control  the  manipulators  and  object  together  to  yield  a  specified  object  position  and 
internal  force  trajectory  [32].  Yoshikawa  et  al.  have  proposed  using  forms  of  the 
so  called  hybrid  position/force  control;  this  method  accomplishes  force  and  posi¬ 
tion  control  along  orthogonal  directions  [61,  44,  63].  Again  this  method  presupposes 
the  existence  of  a  satisfactory  internal  force  trajectory.  Michelman  and  Allen  have 
proposed  and  demonstrated  an  elaboration  of  the  hybrid  scheme  with  the  use  of  a 
blend  of  predefined  manipulative  behaviors  to  accomplish  a  desired  object  manipu¬ 
lation  [34].  The  predefined  manipulative  behaviors  enforce  the  partition  of  the  task 
space  into  force  or  position  controlled  directions.  However,  this  method  also  requires 
knowledge  of  the  internal  force  requirements  necessary  to  maintain  contact  stability 
while  implementing  the  manipulation. 

A  common  thread  throughout  the  various  control  architectures  is  the  require¬ 
ment  of  contact  stability  and  the  need  for  commanded  manipulator  contact  forces. 
A  stable  contact  is  one  in  which  the  applicable  frictional  constraints  are  satisfied.  In 
other  words, 

(1.1) 

where  f„  is  the  normal  contact  force  component,  //  is  the  coefficient  of  static  friction, 
and  ft  is  the  tangential  force  component,  assuming  a  Coulomb  friction  model.  The 
vector  of  generalized  contact  forces,  F,  is  related  to  the  object  wrench,  Q,  where  the 
wrench  is  a  vector  of  object  forces  and  moments  at  a  specified  point,  through  the 
grasp  matrix  [7,  38,  32,  25].  The  grasp  matrix,  W,  maps  contact  forces  to  object 
wrench, 

Q  =  WF  (1.2) 
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where  W  is  a  matrix  containing  elements  of  the  contact  positions,  in  the  object  frame. 
Full  column  rank  of  W  implies  the  grasp  is  one  of  force  closure.  A  force  closure  grasp 
is  one  in  which  any  object  wrench  may  be  resisted  by  some  combination  of  contact 
forces,  i.e.  a  solution  exists. 

The  kinematic  solution  commonly  used  to  determine  F  decomposes  contact 
forces  into  two  orthogonal  sets. 


F  =  Fea:t  +  Fmt  (1-3) 

This  is  usually  accomplished  through  the  use  of  a  generalized  inverse  of  the  grasp 
matrix  W.  A  common  definition  of  external  forces,  Fext:  are  those  forces,  the  sum  of 
which  oppose  the  resultant  wrench  applied  to  the  object  in  the  course  of  accelerating 
the  object  or  reacting  forces  applied  to  the  object.  Internal  forces,  Fint,  are  those 
forces,  the  sum  of  which  produce  no  net  wrench  on  the  object  but  which  serve 
to  achieve  contact  stability  [39,  25,  35].  Thus,  the  external  forces  are  dictated  by 
the  desired  object  behavior,  while  the  internal  forces  are  dictated  by  the  need  to 
maintain  stable  contacts  on  the  object.  Yoshikawa  et  al.  have  defined  grasping  and 
manipulative  forces  similar  to  the  internal  and  external  forces  described  above  [63]. 

Some  authors  propose  using  geometric  properties  of  the  grasp  configuration  to 
determine  internal  forces;  these  solutions  are  not  general  in  nature  [47,  63,  10,  21, 
24].  Park  and  Starr  have  proposed  calculating  the  internal  force  for  a  given  grasp 
configuration  and  scaling  this  solution  as  necessary  to  satisfy  contact  stability  [43]. 
Many  authors  refer  to  optimal  contact  forces.  Some  choose  this  to  mean  that  the  final 
contact  force  solution  lies  as  far  from  the  constraint  boundaries  as  possible  [25].  This 
definition  of  optimal  solution  suffers  on  two  counts.  First,  since  this  solution  entails 
having  larger  forces  than  those  required  for  contact  stability,  power  requirements  for 
the  manipulator  will  be  higher  than  necessary.  Second,  small  errors  in  the  direction 
of  the  contact  forces,  introduced  by  geometric  uncertainties,  may  generate  relatively 
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large  deviations  from  the  desired  object  acceleration  [15].  Nakamura  has  described 
this  effect  as  similar  to  tightly  squeezing  a  wet  bar  of  soap  [40].  Kumar  and  Waldron 
have  proposed  seeking  solutions  where  all  contact  forces  pass  through  a  point  of 
concurrence.  This  point  is  chosen  to  be  the  centroid  of  the  contact  points  [30].  Orin 
and  Oh  have  proposed  a  solution  based  on  the  minimization  of  the  grasp  power  and 
normal  force;  by  linearizing  the  constraints,  they  propose  to  use  linear  programming 
methods  to  solve  the  problem  [42]. 

Nakamura,  Yoshikawa,  and  other  authors  have  defined  optimal  contact  forces 
as  those  which  seek  to  minimize  the  Euclidean  norm  of  the  contact  forces  while 
satisfying  contact  stability  requirements  [38,  63].  This  objective  function  does  not 
suffer  from  the  previously  mentioned  problem  of  squeezing  too  tightly,  though  the 
frictional  constraints  must  be  shifted  inward  to  allow  some  measure  of  solution  ro¬ 
bustness  in  the  face  of  unknown  disturbances.  If  one  seeks  the  minimum  Euclidean 
norm  of  the  contact  forces,  the  pseudoinverse  or  weighted  pseudoinverse  solution  may 
be  used  for  the  calculation  of  external  contact  forces.  This  solution  will  provide  the 
minimum  Euclidean  norm  of  the  external  forces.  However,  the  solution  will  almost 
always  violate  the  contact  frictional  constraints.  The  pseudoinverse  solution  has  two 
parts:  the  actual  pseudoinverse  of  the  grasp  coefficient  matrix  multiplied  with  the 
resultant  force  vector  and  the  matrix,  whose  columns  span  the  null  space  of  the 
grasp  coefficient  matrix,  multiplied  with  an  arbitrary  vector  which  then  defines  the 
internal  force  [43,  47].  It  is  the  internal  force  components  which  must  be  added  to 
the  external  force  components  to  achieve  a  valid  solution.  The  pseudoinverse  solu¬ 
tion  tends  to  distribute  the  object  wrench  equally  between  the  contacts;  the  internal 
forces  (null  space  solution)  modify  the  resultant  force  at  each  contact  so  as  to  satisfy 
friction  constraints. 

For  gripped  contacts  (i.e.  contacts  with  no  frictional  constraints),  the  pseudoin¬ 
verse  solution  is  sufficient  for  implementation  of  a  manipulation  scheme  [3,  6,  56,  59]. 
However,  contacts  involving  friction  require  further  constraints  on  the  solution. 
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Specifically,  that  the  contact  forces  must  lie  in  the  friction  cone.  This  additional 
constraint  requires  the  solution  of  a  non-linear  programming  problem,  which  can 
be  accomplished  in  an  iterative  manner  for  planar  problems,  as  proposed  by  some 
authors  [38].  As  mentioned  earlier,  the  constraints  on  the  Euclidean  norm  solution 
should  be  modified  to  generate  some  measure  of  contact  stability  [39].  This  method  is 
generally  not  satisfactory  with  regards  to  solution  speed  and  has  not  been  effectively 
demonstrated  [37,  30]. 

Recently,  Buss  et  al.  have  proposed  that  this  problem  be  viewed  as  a  linearly 
constrained  semidefinite  programming  problem  for  which  there  are  globally  exponen¬ 
tially  convergent  solutions  via  gradient  flows  [9].  The  algorithm  input  requires  an 
initial  solution  satisfying  the  frictional  constraints  in  order  for  the  method  to  render 
a  valid  solution.  The  objective  function  is  a  mix  of  the  sum  of  the  contact  normal 
forces  and  a  quantity  tending  to  infinity  for  contact  forces  on  the  edge  of  the  friction 
cone.  This  method  also  incorporates  weighting  matrices  which  allow  the  qualitative 
weighting  of  the  objective  function.  The  method  is  reported  to  have  real-time  solu¬ 
tion  capabilities.  Holzmann  and  McCarthy  have  also  recently  proposed  computing 
the  friction  forces  associated  with  a  three-fingered  grasp  using  a  genetic  algorithm 
[22].  Other  recent  papers  include  the  impressive  demonstration  of  a  robust  internal- 
force  based  impedance  control  method  accomplished  by  Bonitz  and  Hsia,  which  also 
requires  the  specification  of  the  internal  forces  required  to  maintain  contact  stability. 
[8].  Sinha  and  Abel  have  proposed  using  elastic  modeling  of  the  manipulator /object 
interface  in  order  to  avoid  using  the  common  Coulomb  friction  constraints  [50]. 

1.3  Problem  Statement 

The  focus  of  this  research  is  the  development  of  a  real  time  capable,  spatial  con¬ 
tact  force  assignment  method  for  determining  the  individual  contact  contributions, 
specifically  the  internal  force  contributions,  for  the  manipulation  of  a  redundantly 
grasped  object.  Fast  solution  speed  is  important  since  ideal  internal  force  magnitudes 
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will  vary  with  changing  external  load  on  the  grasped  object.  Solutions  qualitatively 
similar  to  optimal  solutions  based  on  the  minimization  of  the  Euclidean  norm  of  the 
contact  forces  are  sought.  Additionally,  the  solution  should  be  flexible  with  regards 
to  robust  contact  stability  requirements  and  allow  implicit  control  of  the  available 
internal  forces. 

1.4  Proposed  Solution 

Some  scientists  have  proposed  a  reaction  based  control  scheme  for  basic  ar¬ 
tificial  life  forms  [54].  This  idea  was  born  of  the  need  to  simplify  the  architecture 
of  decision  making  for  artificially  intelligent  agents.  Rather  than  having  a  large 
centralized  knowledge  based  inference  engine  forced  to  maintain  a  consistent  logical 
model  of  the  outside  world,  subsumption  architecture  relies  on  a  decentralized  reac¬ 
tionary  control  scheme  with  the  intervention  of  higher  level  control  in  the  event  of 
conflicting  lower  level  components.  This  architecture  builds  complex  behaviors  from 
conglomerations  of  far  simpler  ones. 

The  philosophy  behind  this  research  is  the  idea  that  perhaps  a  better  way 
to  solve  the  grasping  problem  is  similar  to  the  subsumption  architecture  idea  as 
related  to  artificial  intelligence;  that  is,  to  speed-up  the  problem  solution  by  de¬ 
centralizing  the  solution  generation.  In  the  past,  authors  have  attempted  to  solve 
the  contact  force  assignment  problem  while  considering  all  contacts  simultaneously, 
a  computationally  expensive  proposition.  This  research  will  investigate  the  problem 
solution  through  pair  wise  analysis  of  the  contact  forces.  Thus,  using  the  subsumption 
analogy,  the  proposed  method  is  reactive.  This  proposed  method  also  shares  a  similar 
solution  structure  with  the  subsumption  architecture,  both  methods  lend  themselves 
to  parallel  processing. 

The  proposed  hand  kinematic  solution  is  for  the  spatial  problem  of  grasping  a 
rigid  object  by  multiple  finger  contacts  stably.  The  contacts  are  assumed  to  be  fric¬ 
tional  fingertip  point  contacts  which  can  achieve  arbitrary  forces.  Also,  the  contact 
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geometry,  friction  constraints,  and  commanded  object  wrench  are  assumed  known. 
The  solution  involves  the  use  of  an  iterative  scheme  which  starts  with  the  pseu¬ 
doinverse  solution  of  the  contact  forces.  During  each  loop  of  the  iterative  portion, 
incremental  changes  in  the  internal  forces  will  be  made  based  on  outputs  from  a 
non-linear  inference  process. 

This  inference  will  be  accomplished  through  the  use  of  a  fuzzy  logic  system; 
though,  as  proved  later,  this  is  not  required  for  solution  convergence.  The  process 
developed  to  calculate  the  valid  contact  forces  will  be  referred  to  as  the  fuzzy  logic 
reactive  system  (FLRS).  The  fuzzy  inference  portion  of  FLRS  is  comprised  of  a 
knowledge-based  system  consisting  of  IF... THEN  rules  associated  with  vague  predi¬ 
cates  and  a  fuzzy  logic  inference  mechanism.  The  definition  of  the  input  parameters 
and  output  values  are  based  on  the  heuristic  pair  wise  evaluation  of  the  contact 
forces  and  how  the  shared  internal  force  may  be  used  to  satisfy  the  contact  con¬ 
straints  efficiently.  Thus,  instead  of  globally  driving  all  contact  forces  to  minimize 
some  objective  function,  each  internal  force  is  evaluated  separately  and  are  then 
combined  to  form  the  overall  incremental  change  in  contact  forces. 

Once  a  basic  system  has  been  demonstrated,  one  may  use  one  of  many  auto¬ 
mated  adaptive  techniques  to  tune  the  parameters  controlling  the  mapping  to  achieve 
better  results  [12,  16,  31].  An  alternative  approach  would  be  to  learn  the  FLRS  pa¬ 
rameters  from  sets  of  approved  input-output  data,  much  like  the  training  of  artificial 
neural  networks  [46,  55,  53]. 

1.5  Solution  Validation 

The  FLRS  system  developed  will  be  validated  on  several  levels. 

Criteria  for  solution  convergence  of  force  closure  grasps  (grasps  with  stable  solutions 
to  any  commanded  object  wrench),  will  be  shown  for  both  the  two  and  m- 
contact  cases. 
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The  FLRS  algorithm  will  be  compared  to  an  exact  solution  as  proposed  by  Naka¬ 
mura  for  a  two  contact  case.  The  FLRS  method  will  also  be  compared  with 
numeric  approximations  of  the  Nakamura  method,  which  determines  contact 
forces  that  satisfy  the  frictional  constraints  and  minimize  the  Euclidean  norm 
of  the  contact  forces  through  an  optimization  algorithm.  The  examples  will 
consist  of  various  spatial  grasp  configurations  with  a  range  of  commanded  ob¬ 
ject  wrenches.  A  comparison  of  the  contact  forces  will  be  made  along  with 
the  floating  point  operations  required  to  calculate  the  solution.  The  numerical 
environment  used  for  the  comparison  will  be  MATLAB.  The  specific  numeric 
optimization  algorithm  used  will  be  the  CONSTR  function.  This  function 
solves  the  constrained  optimization  problem,  in  Kuhn- Tucker  form,  using  se¬ 
quential  quadratic  programming  methods. 

A  test  of  the  FLRS  real  time  capability  will  be  accomplished  be  translating  the 
algorithm  to  C  and  compiling  the  code  for  use  by  the  Chimera  real  time  oper¬ 
ating  system.  The  code  will  be  executed  on  a  Motorola  68030  microprocessor 
on  a  VMEbus  card. 
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II.  Background 


2.1  The  Multirobot  Coordination  Problem 

As  mentioned  in  the  introduction,  many  authors  have  reported  various  levels  of 
success  in  developing  control  algorithms  to  coordinate  multiple  robotic  mechanisms. 
The  following  development  of  an  overall  control  strategy  is  the  concept  proposed 
by  Nakamura  [40].  As  mentioned  in  Chapter  I,  there  are  other  architectures,  each 
requiring  contact  forces  which  satisfy  both  contact  stability  and  object  manipulation 
requirements.  The  Nakamura  architecture  is  representative  and  is  presented  here  to 
define  the  context  in  which  this  research  in  contact  force  allocation  can  be  used. 
This  research  is  applicable  to  other  architectures  as  well.  Nakamura  proposes  an 
architecture  which  explicitly  controls  both  the  manipulator  and  the  object  as  the 
basis  for  solving  the  general  precision  grasping  and  manipulation  problem. 

Using  the  notation  of  Nakamura,  the  following  is  a  development  for  coordi- 
native  manipulation  of  a  grasped  object  by  m  robotic  mechanisms,  as  shown  in 
Figure  2.1.  The  basic  assumptions  in  this  development  are: 

•  Grasped  object  is  rigid 

•  Position  of  the  contacts  on  the  object  are  known 

•  Tangent  plane  of  the  object  surface  at  the  points  of  contact  are  known 

•  Object  mass  is  known 

•  Object  velocity  can  be  measured 

•  Contact  configuration  provides  force  closure  grasp 

•  Manipulator  end  point  makes  point  contact  with  the  surface  of  the  object 

•  End  points  do  not  move  on  or  away  from  the  surface  of  the  object 

•  Gravity  vector  is  known 


2-1 


Figure  2.1  Model  and  nomenclature  for  rigid  object  grasped  by  m  manipulators 

These  assumptions  allow  us  to  consider  contacts  which  produce  only  forces  on 
the  grasped  object  and  are  capable  of  producing  an  arbitrary  object  wrench.  The 
object  wrench,  Q,  at  the  object  center  of  mass  consists  of  forces,  fobj,  and  moments, 
not,;')  due  to  the  contact  forces  and  may  be  represented  by: 

m 

^  +  (2.1) 

i=\ 

m 

fi)  (2-2) 

t=i 

where  f*  are  the  contact  forces,  rriobj  is  the  mass  of  object,  g  is  the  gravitational 

acceleration,  and  p,-  is  the  position  of  the  contact  in  the  object  frame.  The 

equations  of  motion  due  to  the  contact  forces  may  be  represented  by  the  Newton- 
Euler  equations: 

mobj'r  =  fobj  (2.3) 
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Iw  +  a;  X  (la;)  =  Uobj 


(2.4) 


where  f  is  the  second  derivative  of  the  position  vector  of  the  object  with  respect 
to  some  inertial  frame,  I  is  the  moment  of  inertia  matrix,  and  co  is  the  rotational 
velocity  vector. 


The  above  equations  are  used  to  formulate  the  equations  of  motion  into  the 
compact  form  below: 

+  Qobj  —  Q  (2-5) 
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where  E„  is  the  order  identity  matrix  and  the  position  and  force  components  1,  2, 
and  3  are  nominally  the  object  x,  y,  and  ar  axes  respectively.  We  could  have  included 
an  external  applied  force  and  moment  in  this  development.  Equations  2.1  through 
2.5  would  have  reflected  this  change  in  the  applied  wrench;  and  would  have  been 
similar  to  the  development  accomplished  by  Schneider  and  Cannon  [49].  However, 
for  purposes  of  putting  this  research  in  context,  that  addition  is  not  necessary. 

If  the  desired  object  trajectory  is  given  by  (j)d{t)  6  3^®;  then,  if  the  net  wrench 

Q  =  Qobj  +  Io6j{^d  +  —  ^)  +  ^2i<l>d  ~  <l>)}  (2.16) 

is  applied  to  the  grasped  object,  the  object’s  motion  will  be  governed  by 

C<i>d  -  h  +  Ki(<^d  -  ^)  +  K2((^d  -  (f>)  =  0  (2.17) 

where  Ki  and  K2  are  constant  coefficient  matrices  chosen  to  guarantee  asymptotic 
convergence  and  (f)  is  the  actual  object  trajectory.  Equation  2.17  indicates  that  the 
actual  object  trajectory  converges  to  the  desired  object  trajectory.  This  behavior 
translates  into  dynamically  stable  response  to  commanded  object  trajectories  and 
is  referred  to,  by  Nakamura,  as  object  stability  [40].  For  this  control  scheme  to  be 
implemented,  one  must  have  <^,  and  the  contact  forces  necessary  to  generate  the 
object  wrench,  Q. 

The  dynamic  portion  of  this  control  problem  seeks  to  generate  the  generalized 
joint  forces  necessary  to  drive  each  manipulator  through  the  commanded  trajectory. 
Below,  the  position  control  algorithm  is  formulated.  The  acceleration  of  any  contact 
point  is: 

r,- =  r +  ch  X  Pi +u;  X  (w  X  Pi)  (2.18) 
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where  the  terms  r  and  u  denote  the  object  accelerations,  both  linear  and  rotational, 
resulting  from  the  object  wrench,  Q.  These  may  also  be  represented  as: 


—  +  Ki((^d  —  <^)  +  —  <l>) 

~  ^obj  (Q  ~  Qo6i) 


(2.19) 

(2.20) 
(2.21) 


The  generalized  manipulator  joint  force  which  results  in  end-effector  r,  , 
when  the  robot  is  not  making  contact  with  the  object,  is  Tip.  This  value  is  the 
trajectory  following  generalized  force  associated  with  non-contact  manipulators. 

Tip  =  Bi{6i)eiCMD  +  C.iOi,  9,)e  +  g^iOi)  (2.22) 


where 


OiCMD 


(2.23) 


and  Jj,  Dj,  Cj,  and  gi  denote  manipulator  Jacobian,  inertial,  Coriolis  and  centrifugal, 
and  gravity  terms  respectively  for  the  manipulator,  in  terms  of  the  generalized 
manipulator  coordinate  vector  9i.  This  relationship,  when  coupled  with  an  error 
reducing  feedback  in  position  and  velocity,  is  referred  to  as  resolved  acceleration 
control  [17]. 


Now  consider  the  generalized  force  required  of  the  manipulator  to  apply 
the  end-effector  force  fj-  statically  to  the  grasped  object.  This  generalized  force  is 
denoted  r,/.  Compute  r,/  by  multiplying  the  transposed  Jacobian  matrix  of  the 
manipulator  by  the  end-effector  force  f,-.  According  to  d’Alembert’s  principle,  the 
net  generalized  force  r,-  required  to  manipulate  the  object  is  the  sum  of  Tip  and  r;/,  as 
shown  in  Figure  2.2  [40].  The  symbol  represents  the  actual  contact  force  applied 
to  the  object  by  the  manipulator. 
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Figure  2.2  Feed  forward  portion  of  object  manipulation  controller 
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An  important  aspect  of  the  control  model  developed  are  the  assumptions  that 
the  contact  forces  commanded  can  in  fact  be  delivered  by  the  grasping  robot  manip¬ 
ulators  and  that  the  object  position  and  velocity  may  be  accurately  determined.  The 
challenges  regarding  the  physical  implementation  of  a  robot  capable  of  operating  as 
modeled  is  not  to  be  underestimated.  Two  key  elements,  of  a  physical  implementa¬ 
tion  of  a  grasping  robot,  are  the  development  of  high  fidelity  fingertip  force  sensors 
and  actuators. 

2.2  Grasp  Force  Assignment 

The  ’’Grasp  Force  Assignment”  block  of  Figure  2.2  is  the  keystone  of  the 
grasped  object  controller,  this  portion  is  solely  responsible  for  the  distribution  of 
the  contact  forces  and  the  maintenance  of  contact  stability.  This  block  is  required  to 
operate  in  real-time  for  hardware  implementations  of  this  architecture.  Two  methods 
commonly  referred  to  for  this  purpose  will  be  presented.  The  first  method,  proposed 
by  Nakamura,  attempts  to  find  an  optimal  solution  based  on  the  minimum  Euclidean 
norm  of  the  contact  forces.  The  second  method,  proposed  by  Nagai  and  Yoshikawa, 
places  additional  constraints  on  the  same  optimization  solution  in  order  to  increase 
solution  speed. 

2.2.1  Nakamura’s  Solution.  A  common  method,  for  the  calculation  of  the 
finger  contact  forces  necessary  to  generate  a  given  object  wrench,  is  the  pseudoinverse 
solution  which  is  exemplified  by  the  methods  proposed  by  Nakamura  et  al.  [38].  The 
pseudoinverse  is  one  of  the  infinite  number  of  generalized  inverse  solutions  for  the 
finger  contact  force  vector  F  of  Equation  2.10.  In  general,  the  contact  forces,  F, 
constitute  a  redundant  set  for  the  solution  of  the  equation.  Thus,  a  solution  for  the 
contact  forces  through  a  generalized  inverse  of  the  grasp  matrix  W  will  yield  two 
parts,  as  in  Equation  2.24. 

F  =  W*Q  -1-  Ay  (2.24) 
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where  €  gfjsmxe  jg  pseudoinverse  of  the  grasp  matrix,  W.  The  term  A,  is 
the  matrix  of  the  orthonormals  of  the  null  space  of  W,  and  y  is  an  arbitrary  vector. 
Thus,  Ay  is  an  element  of  the  null  space  of  W,  and  Dj^  =  3m  —  rank{W)  is  the 
dimension  of  the  null  space  of  W.  Many  authors  use  the  Moore-Penrose  inverse,  or 
pseudoinverse,  solution  as  this  solution  has  the  property  that  W^Q  minimizes  the 
Euclidean  norm  of  the  contact  forces  [40]. 

The  term  is  often  referred  to  as  the  external  force  vector,  Fea;^,  since 

this  is  the  only  term  which  contributes  to  the  desired  object  wrench,  Q. 

Fext  =  W*Q  (2.25) 

Fext=  ^  fext^  fext^  •••  ^  ^  (2.26) 

The  other  element  of  the  solution,  Ay,  cannot  contribute  to  the  net  wrench  and  is 
thus  referred  to  as  the  internal  force  vector,  Fint. 

Fint  =  Ay  (2.27) 

Fint  —  ^  ^intl  •••  ^  €  3?^"  (2.28) 

where 

m 

iinti  =  ^  iintij  (2.29) 

and  iintij  is  the  internal  force  from  contact  i  to  contact  j.  Figure  2.3  illustrates 
the  difference  between  the  internal  and  external  forces.  Internal  force  pairs  lie  on 
a  line  connecting  any  two  contact  points;  thus  their  directions  are  dictated  by  the 
geometry  of  the  contact  points,  W.  The  external  force  directions  are  dictated  by 
both  the  contact  geometry,  and  the  object  wrench,  Q.  As  Figure  2.3  illustrates, 
the  effect  of  the  pseudoinverse  solution,  for  determining  Fext,  is  to  distribute  the 
external  contact  forces  so  as  to  resist  a  given  object  wrench  in  a  minimum  norm 
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Contact  i 


sense.  For  rigid  contact  grasp  problems,  where  the  contacts  are  fixed  to  the  object, 
the  W’^Q  solution  is  entirely  effective  and  reasonable.  However,  for  contacts  which 
rely  on  maintenance  of  contact  forces  that  satisfy  friction  constraints,  this  solution 
will  almost  always  violate  frictional  constraints,  as  illustrated  by  external  contact 
forces  fextk  and  fextj,  in  Figure  2.3. 

In  order  to  satisfy  the  frictional  constraints,  illustrated  in  Figure  2.4,  the  solu¬ 
tion  derived  from  the  pseudoinverse  must  be  modified.  The  modifiers  to  the  W’^Q 
solution  are  the  internal  forces.  Nakamura  determines  these  internal  forces  while  con¬ 
tinuing  to  minimize  the  Euclidean  norm  of  the  contact  forces.  The  method  outlined 
below  is  acceptable  for  solving  the  simple  two  contact  problem  but  is  prohibitively 
expensive  with  respect  to  computational  time  for  more  complex  problems  [37,  30]. 

Nakamura  formulates  and  solves  the  constrained  optimization  problem  using 
the  Kuhn- Tucker  theorem  [40].  For  the  specified  object  wrench,  Q,  obtain  F  that 


2-9 


Object  Surface 


>'/ 


satisfies 

min||F|| 


(2.30) 


with  the  constraints 

Q  =  WF  (2.31) 

Allfill  Vi  (2.32) 

where  =  1  j  yj\  n}.  We’ve  previously  assumed  that  the  finger  is  capable 
of  applying  an  arbitrary  force  at  the  fingertip  and  that  force  closure  exists.  The 
constraints  of  Equation  2.32  may  be  written  as: 


where 


9i{y) 


5't(y)  <  0  i  =  l,...,2m 


T'  f. 


i  =  1,  .  .  .  ,  m 

► 

i  ==  m  +  1, . . . ,  2m 


(2.33) 


(2.34) 
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and  B,  =  —  e^vie^i-  Since  ||F||  is  a  positive  function,  we  may  declare  an 

equivalent  objective  function  to  Equation  2.30  as: 

min/i(y)  (2.35) 

where 

Ky)  =  F^F  (2.36) 

Thus  the  problem  of  determining  the  contact  forces  which  minimize  ||F||  has  been 
reduced  to  minimizing  a  quadratic  function  with  linear  and  quadratic  inequality 
constraints.  To  determine  the  unknown  variable  y,  we  apply  the  Kuhn-Tucker  The¬ 
orem  [40].  An  augmented  objective  function,  e(y.  A),  is  formed  using  the  Lagrange 
multipliers  A  =  ^  Ai  A2  •  •  •  \2m  ^  ^ 

2m 

£(y.  A)  =  h{y)  +  ^  A,-^,(y)  (2.37) 

j=i 

where  A,-  >  0.  The  Kuhn-Tucker  Theorem,  and  the  fact  that  the  augmented  objective 
function  can  be  shown  to  be  a  convex  function  with  respect  to  y,  leads  to  the 
necessary  and  sufficient  conditions  for  the  global  minimum  of  e(y,A),  and  thus 
||F||  [40], 


1 

^  y=yO,A=A^ 

(2.38) 

2m 

E  =  0 

t  =  l 

(2.39) 

where  y°  and  A°  are  the  values  of  y  and  A  which  minimize  the  objective  function 
and  satisfy  the  constraints. 

This  formulation  solves  for  all  internal  forces  simultaneously  which  leads  to  ex¬ 
tremely  large  polynomial  objective  functions  for  even  modest  contact  configurations. 
The  number  of  internal  force  pairs,  TVj,  to  simultaneously  specify  for  a  solution  is 
(m^— m)/2.  For  a  five  contact  problem,  ten  internal  forces  must  be  determined.  Con- 


sidering  the  case  of  five  contacts  on  an  object,  m  =  5,  ||Fj  ||^  is  a  15  term  quadratic 
equation  in  terms  of  the  four  relevant  internal  force  magnitudes.  The  number  of 
independent  terms  in  |1F||^  is  Np2  =  1  +  Ni  +  (N]  —  iV/)/2,  or  56  terms  for  the  five 
contact  problem.  Simultaneously  determining  a  minimum  norm  solution  among  all 
possible  solutions  is  a  daunting  and  time  consuming  task.  Problems  with  more  than 
two  contacts  are  generally  solved  using  numeric  optimization  methods.  The  state¬ 
ment  of  conditions  for  the  global  minimum  allow  numeric  optimization  algorithms 
to  be  guaranteed  to  converge  to  the  global  minimum. 

2.2.2  Exact  Solution  Example  .  The  exact  solution  using  the  previously 
outlined  method  can  be  determined  for  simple  two  contact  problems.  In  this  case 
the  problem  must  be  reduced  to  a  planar  one  as  the  grasp  matrix,  W,  is  not  full 
rank  for  the  spatial  case  and  thus  force  closure  will  not  exist.  The  elements  of 
Equation  2.24  are  thus  reduced  in  dimension.  The  unknown  variable  y  is  a  scalar  for 
this  case,  indicating  that  we  have  only  a  single  internal  force  with  which  to  apply  to 
the  external  force  solution  to  enforce  contact  constraints. 

Figure  2.5  illustrates  the  geometry,  axis  system,  and  commanded  object  wrench 
of  a  two  contact  example,  in  which  the  grasped  object  is  an  ellipse.  The  contact 
inward  pointing  normal  directions  are  the  negative  of  the  respective  position  vectors. 
Pos  is  the  matrix  of  position  vectors,  in  column  order. 
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Pos  = 


1  -1 

0  0 


(2.40) 


Let  the  coefficient  of  static  friction  for  both  contacts,  //,  be  0.4  and  the  commanded 
planar  object  wrench,  Q,  be; 


Q=<  1 


(2.41) 


The  grasp  matrix,  W,  and  the  orthonormals  of  the  null  space  of  W,  A,  from  Equa¬ 


tion  2.40  are: 


10  10 

W  =  0  10  1  A  = 

0  10-1 


-0.7071 


0.7071 


(2.42) 


The  pseudoinverse  of  W  is. 


W#  = 


0.5  0  0 

0  0.5  0.5 

0.5  0  0 

0  0.5  -0.5 


(2.43) 


and  Equation  2.24  may  now  be  written  as: 


’0.5  0  0 

0  0.5  0.5 

0.5  0  0 

0  0.5  -0.5 


1  + 


-0.7071 


0.7071 


-0.7071 


0.7071 


y  (2.44) 


Clearly,  the  sum  of  the  external  contact  forces  and  moments  equal  the  commanded 
object  wrench. 
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By  applying  the  only  internal  force  available,  the  net  contact  forces  will  move 
inward  toward  the  friction  cones  as  the  internal  force  magnitude,  y,  is  increased. 
The  contact  force  for  the  second  contact,  {2  will  thus  move  inside  the  friction  cone 
before  fi.  Therefore,  the  constraint  equation  g2{y)  <  0  and  thus  A2  =  0.  We  also 
know  that,  in  general,  both  contacts  will  be  exerting  force.  Therefore,  g3{y)  <  0  and 
g4{y)  <  0  which  requires  A3  =  0  and  A4  =  0.  For  this  solution  to  be  a  minimum 
norm  solution,  one  contact  force  must  be  on  the  friction  cone,  therefore  gi{y)  =  0 
Thus,  we  have  reduced  the  five  equations  from  the  Kuhn- Tucker  formulation  to  two 
equations,  with  two  unknowns,  y  and  Ai. 

The  components  of  the  augmented  objective  function  of  Equation  2.37  may 
now  be  formed. 

h{y)  =  (0.5  -  O.lOllyf  -f  0.5^  +  (0.5  -f  O.lOllyf  +  0.5^  (2.45) 

=  1  +  2/'  (2.46) 


and 


therefore 


e/vi  = 


n 

1 

0 

1— 1 

1 

1 _ 

1 0  j 

L  0  /32J 

gi{y)  =  (0.5  -  0.707l2/)2(/32  _  i)  +  o.5'(^') 
Equations  2.38  and  2.39  may  now  be  constructed, 


(2.47) 


(2.48) 


de 

dy 


y=y°  ,A=A^ 


2m 


=  2y°  +  2A°  [(-0.7071)(0.5  -  0.7071y°)(/3^  -  1) 


E  =  K  [(0-5  -  0.7071^)2(^2  _  1)  ^  0  25/3' 
2  =  1 


i)j  =0 

(2.49) 

2]  =0 

(2.50) 
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and  y°  may  be  solved  for: 


y°  =  0.7071  + 


1 


.5  -  0.5 


'  2^2  _ 1 ' 
IF^), 


(2.51) 


For  this  particular  case,  y°  =  2.4744,  and  the  contact  forces  are: 


F  =  ^ 


-1.25 

0.5 

2.25 

0.5 


(2.52) 


For  general  and  realistic  grasping  problems,  at  least  one  contact  force  will  lie  on 
the  friction  cone  (necessary  for  a  minimum  norm  solution),  thus  the  exact  solution 
to  the  two  contact  problem  is  relatively  simple.  Larger  problems  will  in  general 
exhibit  coupling  between  Equations  2.38  and  2.39.  Grasp  configurations  may  occur, 
especially  with  large  values  of  /i,  in  which  the  pseudoinverse  solution  does  lie  within 
all  the  contact  constraints;  one  would  then  not  need  any  additional  contact  force 
components. 


2.2.3  Yoshikawa’s  Method  of  Contact  Assignment.  The  normal  component 
of  the  external  force  solution  for  Contact  1,  of  the  two  contact  example  above,  was 
away  from  the  surface.  The  external  contact  force  solution  in  and  of  itself  is  clearly 
not  sufficient  for  frictional  contacts  and  is  somewhat  misleading  in  that  some  internal 
and  external  forces  may  be  equal  and  opposite  producing  no  net  contact  force.  The 
method  proposed  by  Yoshikawa  and  Nagai  attempts  to  address  this  issue  and  to 
speed  up  the  solution  with  additional  constraints. 

The  force  space  is  decomposed  into  manipulative  and  grasping  forces,  similar 
to  the  contact  force  development  of  Kobayashi  [63,  27].  These  forces  are  also  similar 
to  the  internal  and  external  force  definitions  described  earlier.  The  grasping  forces 
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are  defined  as  those  which  lie  in  the  null  space  of  the  grasp  matrix  W  and  satisfy  the 
contact  friction  constraints.  The  manipulating  forces  are  those  which  produce  the 
desired  resultant  wrench,  do  not  act  in  the  inverse  direction  to  the  grasping  force, 
and  are  orthogonal  to  the  grasping  force  components.  It  is  the  additional  constraints 
on  the  directions  of  the  manipulating  forces  which  differentiates  this  method  from 
the  method  proposed  by  Nakamura  et  al.  The  method  is  applicable  for  two,  three, 
and  four  contacts  only.  The  contact  forces  are  separated  into  two  orthogonal  sets: 

F  =  F,+Frr.  (2.53) 

where  and  F„,  =  B^  G  G  3?”,  B,,  G  and 

hm  £  3^^  where  n  =  {1,3,6}  and  /  =  {3,6,6}  for  two,  three,  and  four  contact 
problems  respectively.  The  unknown  variables  are  the  grasp  parameters,  h^. 

The  matrix  B^  is  determined  from  the  contact  geometry.  For  a  three  contact 
problem  of  a  convex  object, 

0  ei3  ei2 

~  ^23  0  621  (2.54) 

©32  ^31  0 

The  matrix  B^  is  a  function  of  both  the  contact  geometry  and  the  commanded 
object  wrench.  Thus  B^  must  be  calculated  for  each  Q. 

0  (1  ~  ^2)G13  ^3^12  Gio  0  0 

Bm  —  ^1623  0  (1  —  ^3)621  0  620  0  (2.55) 

(1  —  ^1)632  ^2^31  0  0  0  630 
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where  ki  are  parameters  which  can  take  on  the  value  of  0  or  1  and  Sij  are  the  basis 
vectors  of  the  manipulating  force  (unit  vectors  from  contact  i  to  contact  j). 


62(2+1)  X  e,(j-4.2) 

(2.56) 

=  ■ 

6i(2+l)  X  e,(,-4.2) 

® 2(2+1)  ^2(2+2)  ^  620 

(2.57) 

Gi(2+2)  =  Cjo  X 

(2.58) 

The  parameters  ki  are  chosen  such  that: 

h^  =  (WB„)-'Q  (2.59) 


where  hmi  >  0. 

The  solution  method  proposed  by  the  authors  requires  either  a  planned  trajec¬ 
tory  of  grasp  parameters,  h^,  which  are  known  to  satisfy  the  frictional  constraints 
or  an  iterative  solution  of  the  unknown  grasp  parameters  [37].  This  method  may  be 
thought  of  as  similar  to  the  method  of  Nakamura  with  additional  constraints  asso¬ 
ciated  with  the  direction  of  the  orthogonal  forces.  However,  the  extra  computations 
associated  with  may  make  this  method  no  faster  than  that  proposed  by  Naka¬ 
mura.  And  like  the  Nakamura  algorithm,  for  problems  with  three  of  more  contacts, 
a  numeric  solution  is  required.  The  proposed  method  has  not  been  extended  to 
consider  over  four  contacts. 

2.2.4  Exact  Solution  Example  .  An  exact  solution  to  the  two  contact  prob¬ 
lem  described  in  Section  2.2.2  can  also  be  calculated  using  the  Yoshikawa  algorithm. 
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For  the  two  contact  problem,  the  matrix  is: 


^21 


-1 

0 

1 

0 


(2.60) 


and  the  unknown  grasp  parameter,  hg,  is  a,  scalar.  The  grasp  parameter  is  analogous 
to  the  internal  force  magnitude,  y,  of  Section  2.2.2.  The  basis  vectors  for  matrix 
are: 


1 

f-il 

fil 

1 

f  0  1 

1 

ei2  =  j 

[  0  J 

.  621  =  1 

i  “  J 

^  eio  =  1 

1->J 

*  620  =  S 

hi 

(2.61) 


and 


B. 


^1^12  6l0  O 

(1  —  A:i)e2i  O  §20 


1  0  0 

0-10 

2  0  0 

0  0  1 


(2.62) 


for  ki  =  —1.  The  vector  may  be  calculated  according  to  Equation  2.59: 


hm  =  (WB,„)-'  Q 


0.3333 

-0.5 

0.5 


The  contact  force  vector,  F  =  ^nMm  +  ^ghg  may  now  be  formed. 


0.3333 

-1 

F  = 

0.5 

+ 

0 

0.6666 

1 

0.5 

0 

(2.63) 


(2.64) 
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where  the  first  term  is  analogous  to  the  external  force  of  Equation  2.44,  though 
different  in  magnitude.  An  objective  function  and  constraint  equation  similar  to 
Equation  2.45  and  2.48  may  be  formed  using  the  above  equation  for  the  contact 
forces.  Solving  the  equivalent  equations  to  Equation  2.49  and  2.50,  the  grasp  pa¬ 
rameter  is  calculated: 


hg  =  0.6666  + 


(0.6666)2-4(0.3333)2(0.5)^ 


^2 


(2.65) 


For  this  particular  case,  hg  =  1.7338,  and  the  contact  forces  are  thus: 


-1.4 


0.5 

2.4 

0.5 


(2.66) 


which  are  greater  than  those  calculated  using  the  method  of  Nakamura.  The  differ¬ 
ence  lies  in  the  calculation  of  the  equivalent  external  force.  The  Nakamura  algorithm 
uses  the  pseudoinverse  which  guarantees  a  least  squares  solution  of  the  external  force, 
while  the  Yoshikawa  method  does  not.  Thus,  we  have  illustrated  for  one  case,  the 
Yoshikawa  solution  will  not  result  in  the  minimum  norm  of  the  contact  forces. 


2.3  Summary 

We  have  reviewed  contact  force  distribution  methods  associated  with  the  pseu¬ 
doinverse  solution,  proposed  by  Nakamura  et  ah,  and  the  method  proposed  by 
Yoshikawa  and  Nagai.  In  general,  the  method  proposed  by  Nakamura  relies  on 
numerical  solution  methods  which  have  been  shown  to  lack  real-time  solution  capa¬ 
bility  [63].  The  Yoshikawa  solution  is  similar,  with  the  addition  of  some  directional 
constraints,  and  additional  computational  burden. 
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III.  Background  to  Fuzzy  Logic 


As  the  previous  chapter  pointed  out,  the  nature  of  the  contact  force  assignment 
problem  is  the  solution  of  a  generalized  inverse  problem.  Unfortunately,  these  solu¬ 
tions  are  not  unique  [40].  The  pseudoinverse  solution  is  a  convenient  and  relatively 
simple  inverse  to  pursue.  A  satisfactory  solution,  from  the  point  of  view  of  real  time 
manipulator  control,  of  this  problem  with  conventional  mathematical  tools  is  proba¬ 
bly  not  a  feasible  solution  in  the  near  future  [37].  As  alluded  to  in  the  introductory 
chapter,  the  proposed  research  is  grounded  in  artificial  intelligence  methods  to  solve 
this  problem,  specifically  fuzzy  logic. 

3.1  Logic  and  Ambiguity 

Binary  logic  was  codified  by  Aristotle  through  his  laws  of  logic  based  upon  the 
concept  of  A  and  not-A;  the  Law  of  the  Excluded  Middle.  A  particle  cannot  be  both 
in  A  and  in  not-A;  there  is  no  ambiguity  about  A.  In  a  purist  mathematical  sense 
this  is  correct.  However,  in  order  to  maintain  this  boundary  of  what  is  and  what 
is  not;  artificial  constraints  must  be  applied.  It  is  these  artificial  constraints  which 
divorce  mathematical  rules  and  logic  from  human  perception  of  reality.  For  instance, 
a  heap  of  sand  may  be  defined  as  a  group  of  small  particles,  in  close  proximity,  with 
at  least  x  number  of  particles  and  not  one  less.  Thus  we  have  defined  a  heap  and  a 
non-heap;  mathematically  we  can  now  differentiate  between  two  piles  of  sand.  One 
pile  having  x  particles  is  a  heap,  and  one  pile  having  x  —  \  particles  is  not  a  heap. 
The  number  of  particles  required  to  define  a  heap  is  only  one  aspect  of  the  previous 
definition  which  is  open  to  ambiguity. 

This  black  and  white  world  of  sets  is  particularly  troublesome  for  rule-based 
systems,  such  as  knowledge-based  artificial  intelligence  systems.  Rules  based  on 
absolute  dichotomy  are  relatively  inefficient  since  definitions  must  be  numerous  and 
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overly  precise.  This  precision  leads  away  from  generality  which  should  be  a  goal  for 
such  a  system  [18]. 

A  number  of  modifications  to  the  bivalent  logic  of  Aristotle  have  been  pro¬ 
posed  and  used  over  the  years.  These  modifications  include  the  discretization  of 
the  bivalent  logic.  A  number  of  different  theories  of  multivalued  logics  were  pro¬ 
posed  in  the  early  twentieth  century  by  Bochvar,  Kleene,  Heyting,  Reichenbach, 
and  Lukasiewicz.  Generally,  these  theories  were  initially  proposed  to  deal  with  the 
Heisenburg  uncertainty  principle  in  quantum  mechanics  [29].  Lukasiewicz  developed 
the  first  A^-valued  logic  in  the  1930’s,  where  N  >2  .  Thus,  for  N  =  2,  the  standard 
bivalent  logic  applies.  In  an  A^-valued  logic,  the  truth  values  are  assumed  evenly 
distributed  along  the  closed  interval  [  0  1  ]•  The  nomenclature  associated  with  the 
multivalued  logics  is  Lj^.  Thus,  an  logic  consists  of  truth  values  of  {  0  1/2  1  }• 
In  this  logic,  a  statement  could  be  half-true.  The  discretization  was  eventually  ex¬ 
panded  to  A"  =  00,  where  truth  values  were  the  real  numbers  in  the  closed  interval 
[  0  1  ]•  However,  the  purpose  behind  the  expanded  logics  still  remained  the  same 
as  the  earlier  bivalent  logic,  exact  reasoning  [18]. 

In  the  1930’s  another  logician,  Bertrand  Russell  used  the  term  ’’vagueness”  to 
describe  multivalence.  In  1937,  quantum  philosopher  Max  Black  published  a  paper 
on  ’’vague”  sets,  which  were  essentially  the  same  as  the  ’’fuzzy”  sets  we  speak  of 
today  [29].  If  the  scientific  community  had  lent  more  time  to  the  concepts  proposed 
by  Black,  we  might  have  had  vague  logic  50  years  ago.  Lotfi  Zadeh  first  proposed 
the  basic  theory  of  fuzzy  sets  in  his  1965  paper  ’Fuzzy  Sets,’  in  which  he  referred  to 
the  work  of  Kleene.  The  paper  on  fuzzy  sets  later  served  as  the  basis  for  the  fuzzy 
logic  now  common  today.  The  math  of  fuzzy  sets  used  the  same  algebra  worked  out 
by  Lukasiewicz  in  the  1930’s,  though  today  it  has  expanded. 

The  essential  premise  of  fuzzy  sets  is  the  contradiction  of  Aristotle’s  Law  of 
Excluded  Middle.  A  particle  may  exhibit  a  level  of  membership  in  one  or  more  sets. 
Thus  a  half  eaten  apple  has  a  level  of  membership  in  the  set  of  apples  and  in  the 
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sets  of  non-apples.  Depending  on  the  characteristics  of  the  domain  of  interest,  the 
summation  of  the  degree  to  which  the  half-eaten  apple  belongs  to  other  sets  may 
exceed  unity.  This  is  a  difference  between  fuzzy  sets  and  other  exact  multivalence 
set  theories. 

3.2  Fuzzy  Sets  and  Fuzzy  Logic 

Zadeh  showed  how  the  concepts  of  fuzzy  sets  and  fuzzy  logic  mimic  the  linguis¬ 
tic  constructs  of  human  language  and  how  such  concepts  may  allow  reasoning  akin  to 
humans  [28].  Rules  based  on  ambiguous  linguistic  concepts  may  be  used  with  fuzzy 
logic;  where  fuzzy  logic  provides  the  numeric  mechanism  by  which  degrees  of  truth 
of  premises  (antecedents)  are  manipulated  through  the  rules  to  provide  conclusions 
(consequents). 

The  definition  of  membership  is  the  degree  of  the  truth  of  the  statement:  x  is 
a  member  of  fuzzy  set  A.  The  membership  value  is  in  the  closed  interval  [  0  1  ],  0 
indicating  complete  falsehood  and  1  indicating  complete  truth.  The  finite  fuzzy  set 
A  can  be  represented  as  the  set  of  ordered  pairs  A  =  (x,77^(x))|xGA’,  where  X 
is  the  domain  and  7?a(x)  is  the  membership  function.  Figure  3.1  demonstrates  the 
difference  between  fuzzy  and  binary  (crisp)  sets  by  plotting  the  membership  functions 
for  the  sets  of  medium  and  tall  people.  There  are  many  subjective  definitions  of  the 
makeup  of  a  fuzzy  set  which  comprise  the  domain  of  interest.  In  Figure  3.1  sets  of 
both  medium  and  tall  people  are  described  over  the  domain  of  height.  The  support 
of  each  fuzzy  set  constitutes  the  portion  of  the  domain  where  the  membership  values 
are  greater  than  zero.  A  common  approach  is  the  use  of  triangular  membership 
functions  as  this  helps  to  speed  up  the  numeric  process  [58]. 

Fuzzy  logic  is  concerned  with  approximate  reasoning  vathev  than  exact  multival¬ 
ued  logic  as  proposed  by  Lukasiewicz  et  al.  Fuzzy  reasoning  consists  of  the  inference 
of  a  possibly  imprecise  conclusion  from  a  set  of  possibly  imprecise  premises.  This  is 
the  action  involved  with  most  human  reasoning.  There  are  basically  two  approaches 
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Crisp  Distribution 
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Fuzzy  Distribution 
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Figure  3.1  Conventional  and  fuzzy  sets 


to  using  fuzzy  logic  to  solve  input-output  mapping  problems:  develop  a  logical  rule 
base  similar  to  a  rule  base  a  human  might  use  to  solve  the  same  problem,  or  use 
acquired  data  of  correct  input-output  pairs  to  develop  the  rule  base  by  learning  the 
mapping.  The  learning  method  strictly  uses  the  current  data  to  develop  a  fuzzy  logic 
system  capable  of  generating  the  same  output  as  the  original  system  [55,  28]. 

This  research  will  use  a  heuristic  approach  to  the  fuzzy  logic  rules  and  domains. 
If  somewhat  promising,  future  research  may  strive  for  adaptive  or  learning  systems 
which  seek  to  minimize  some  objective  function.  The  primary  concern  of  this  research 
is  the  development  of  tools  for  quick  numeric  solutions  and  will  not  strive  to  explicitly 
produce  a  mathematically  optimum  solution  of  some  objective  function.  The  rules 
and  the  domains  of  the  relevant  parameters  will  be  derived  in  the  spirit  of  traditional 
expert  systems. 
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Figure  3.2  Fuzzy  logic  system 


3.3  Fuzzy  Logic  Mechanics 

A  basic  fuzzy  logic  system  may  be  described  by  Figure  3.2.  The  inputs  are 
generally  discrete  values  obtained  from  digital  inputs  or  analog  inputs,  the  latter 
obtained  from  A/D  devices.  Note:  Some  fuzzy  systems  have  been  developed  to  take 
advantage  of  direct  analog  inputs.  The  inputs  are  then  categorized  as  fitting  each  of 
the  input  membership  functions  to  some  degree.  This  process  is  commonly  referred 
to  as  fuzzification  [64,  11].  The  membership  functions  which  span  the  input  domain 
are  defined  and  fixed  a  priori  for  non-adaptive  systems.  The  fuzzy  inputs  are  then 
fed  across  the  fuzzy  rule-base,  where  an  inference  is  made  from  each  instance  of  the 
data.  The  inference  mechanism  is  based  on  the  rules  which  form  the  rule-base. 

The  rules  consist  of  fuzzy  antecedents,  aggregation  operators,  and  fuzzy  conse¬ 
quents.  The  antecedents  and  consequents  are  the  fuzzy  input  and  output  sets.  The 
aggregation  operators  are  from  the  t-norm  and  t-conorm  set  of  operators  which  gen¬ 
eralize  the  intersection  (conjunction)  and  union  (disjunction)  operations  respectively 
[57].  An  example  of  a  rule  is: 

IF  A  is  Um  and  B  is  Usi  THEN  C  is  Ua  (3.1) 

where  A  is  the  variable  of  the  domain  Ua,  B  is  the  variable  of  the  domain  Ub,  and 
C  is  the  variable  of  the  domain  Uc-  UAi,  Usi,  and  Ua  denote  specific  member- 
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ship  functions  (fuzzy  variables)  of  their  respective  domains.  Equation  3.1  uses  the 
conjunctive  aggregation  operator,  AND.  Similarly,  OR  is  a  disjunctive  operator. 
The  mechanism  by  which  fuzzy  sets  are  aggregated  is  not  unique.  However,  all  such 
mechanisms  do  fall  within  the  definitions  of  t-norm  and  t-conorm  defined  respec¬ 
tively  as  the  mappings  T  :  [0, 1]  x  [0, 1]  — »•  [0, 1]  and  S  :  [0, 1]  x  [0, 1]  — >  [0, 1]  such 
that  these  mappings  meet  the  following  criteria  [28]: 


r(a,  b)  =  T(b,  a)  S{a,  b)  =  S[b,  a)  Commutativity 

T{a,T{b,c))  =  T{T{a,b),c)  S{a,S(b,c))  =  S{S{a,b),c)  Associativity 

r(a,  b)  >  T(c,  d)  S{a,  b)  >  S{c,  d)  if  a>  c  and  b  >  d  Monotonicity 

T(a,  1)  =  a  S(a,  0)  =  a  Identity 

(3.2) 

where  a  and  b  are  the  degrees  of  membership  in  their  appropriate  membership  func¬ 
tions.  The  Min/Max  operators. 


Max{a,  b) 
Min{a,  b) 


a  +  b-{-\a  —  b\ 

2 


a  +  b  —  \a  —  b\ 

2 


are  commonly  used  t-norm/ t-conorm  mappings  [57]. 

Though  many  other  operators  exist,  Min/Max  will  be  used  for  this  research. 
Thus,  the  conjunctive  operator  is: 


Min{riA{xA),VB{xB))  (3.3) 

where  xa  and  xb  are  the  input  values  associated  with  the  A  domain  and  B  domains, 
respectively;  determines  the  weight  of  the  associated  fuzzy  consequent.  Once  the 
fuzzy  consequent  and  the  weight  associated  with  it  are  determined  for  all  fuzzy  input 
pairs,  one  must  derive  a  non-fuzzy  output.  This  process  is  known  as  defuzzification 
and  again,  there  are  many  methods  used  for  this  purpose  [58,  57,  13].  This  research 
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Figure  3.3  One  dimensional  example 


will  use  the  weighted  average  of  fuzzy  singletons  which  has  a  significant  reduction  in 
computational  costs  compared  with  other  common  methods  [51].  A  summary  of  the 
design  parameters  which  must  be  defined  include:  parameters  which  constitute  the 
inputs,  fuzzy  domains  which  span  the  input  and  output  space,  fuzzy  rule-base,  ag¬ 
gregation  operators,  and  defuzzification  procedure.  A  simple  example  of  this  process 
follows. 


3.4  Example  of  Fuzzy  Control 

A  fuzzy  logic  system  consisting  of  heuristically  derived  rules,  consists  of  a  rule 
base,  which  is  an  order  system  of  relationships  among  the  input  parameters 
and  output  parameters,  n  being  the  number  of  input  parameters  in  the  rule  base. 
Typically,  the  rule  base  may  be  envisioned  as  an  order  matrix  of  cells.  Each 
cell  corresponds  to  a  fuzzy  output  set  given  the  input  fuzzy  arguments.  The  fol¬ 
lowing  example  models  simplistic  position  control  using  acceleration  inputs  for  an 
automobile. 

This  example  will  concern  the  single  dimensional  problem  controlling  an  auto¬ 
mobile  to  a  given  position  through  commanded  changes  in  acceleration.  Assume  the 
input  variables  for  an  automobile  controller  can  be  reduced  to  two  inputs,  distance 
to  the  target,  x  and  current  velocity  v  =  x.  Figure  3.3  illustrates  the  problem.  Let 
the  output  be  the  acceleration  a  =  x.  Assume  the  domains  of  interest  are: 


X 


-10  10 
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Fuzzy  Sets 
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Figure  3.4  Example  fuzzy  antecedent  and  consequent  sets 


V 

a 


-5  5 
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Further,  assume  the  domains  are  divided  among  the  five  fuzzy  sets:  large  negative 
(LN),  small  negative  (SN),  zero  (ZE),  small  positive  (SP),  and  large  positive  (LP). 
These  domains  are  illustrated  in  Figure  3.4.  Using  the  above  assumptions,  the 
structure  of  the  rule-base  is  defined.  The  rule-base  will  consist  of  a  25  element 
matrix,  which  constitutes  all  the  possible  combinations  of  fuzzy  set  inputs.  The 
acceleration  domain  is  made  up  of  fuzzy  singletons  [51].  The  output  membership 
functions  are  unit  impulses,  which  when  coupled  with  a  weighted  average  inference 
process,  is  commonly  referred  to  as  the  Sugeno-style  fuzzy  inference.  The  heuristic 
rules  used  to  determine  the  output  for  each  cell  derive  from  various  permutations  of 
the  rule  below: 
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LN  SN  ZE  SP  LP 
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Figure  3.5  Example  rulebase 


IF  V  is  LP  AND  X  is  SP  THEN  a  is  LN  (3.4) 

The  control  surface  of  these  heuristically  derived  rules,  is  illustrated  in  Figure  3.5. 

The  operation  of  the  inference  mechanism,  where  an  instance  of  inputs  is 
mapped  to  the  output  range  through  the  rule-base,  is  described  next.  Given  an  input 
pair  V  =  —2.0,  x  =  3.3,  the  first  step  in  the  inference  process  is  the  fuzzification  of 
the  inputs.  The  velocity  and  distance  membership  values  are:  {0.0, 0.8, 0.2, 0.0, 0.0} 
and  {0.0,0.0,0.34,0.66,0.0}  respectively.  Or,  2.0)  =  0.8,  r]^^(—2.0)  =  0.2, 
=  0.34,  t]^^{3.3)  =  0.66,  with  the  membership  in  the  other  functions  zero. 
For  each  cell  in  the  rule-base  matrix,  the  weight  of  the  output  cell  is  determined 
by  the  value,  Min{T]v{v),rjx{x)).  Thus,  many  of  the  cells  result  in  zero  output  for  a 
given  input  set  {  a;  v  }■  Figure  3.6  illustrates  the  weight  associated  with  each  of 
the  input  membership  functions.  In  general,  for  two  input  systems,  only  four  rules 
of  the  rulebase  will  have  greater  than  zero  output. 

r]{SN,ZE)  =  Mm(0.8,0.34)  =  0.34 
'r){SN,SP)  =  Afm(0.8, 0.66)  =  0.66 
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Instance  of  Fuzzy  Inputs 


Figure  3.6  Instance  of  x  =  3.3,  and  v  =  —2.0 


ri{ZE,ZE)  =  Mm(0.2,0.34)  =  0.2 
Tj{ZE,SP)  =  Mm(0.2,0.66)  =  0.2 

The  final  output  control  value  is  the  weighted  average  of  the  four  consequents. 

m 

E  ViVai 

y*  =  -  (3.5) 

E  Vai 

i—1 

where  m  is  the  number  of  fuzzy  consequents,  yi  is  the  fuzzy  singleton  value,  rjai 
is  the  weight  of  the  consequent 

The  SIMULINK  model  which  was  used  to  simulate  this  controller  is  illustrated 
in  Figure  3.7  [1,  2].  Two  simulations  are  illustrated  for  two  different  initial  conditions 
in  Figures  3.8  and  3.9. 
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Figure  3.7  Simulink  model  of  1-D  example 


Position  Control,  Rulebase  1 ,  Case  A 


Time 

Figure  3.8  Position  control  results,  Case  A 
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Position  Control,  Ruiebase  1 ,  Case  B 


Figure  3.9  Position  control  results,  Case  B 


The  performance  characteristics  of  the  example  position  controller  can  be  easily 
changed  by  changing  the  values  of  the  cells  in  the  rule-base.  The  modified  rule-base 
along  with  the  change  in  controller  performance  are  illustrated  in  Figures  3.10,  3.11, 
and  3.12 

Note,  this  particular  fuzzy  control  model  functions  similarly  to  a  conventional 
PD  controller.  Simple  fuzzy  control  models  using  inputs  and  outputs  equivalent  to 
errors,  change  in  errors,  and  sum  of  errors  approximate  various  forms  of  PI,  PD,  and 
PID  control  schemes  [45,  48,  57]. 

3.5  Summary 

In  this  chapter,  a  brief  history  of  the  concept  of  fuzzy  sets  and  logic  have  been 
presented.  Also  presented  were  the  mechanics  of  a  simple  fuzzy  logic  controller, 
including  concepts  surrounding  construction  of  the  system.  The  basic  elements  in  the 
fuzzy  controller  include  definition  of  the  input  parameters  in  terms  of  a  fuzzy  domain. 
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Figure  3.10  Modified  rulebase 


Position  Control,  Rulebase  2,  Case  A 


Figure  3.11  Modified  position  control  results,  Case  A 
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Position  Control,  Ruiebase  2,  Case  B 


Figure  3.12  Modified  position  control  results,  Case  B 


the  rule-base  by  which  the  inference  mechanism  can  operate,  and  the  determination 
of  the  final  consequent  response.  These  elements  are  common  to  most  fuzzy  control 
systems.  The  decisions  concerning  the  details  of  the  shape  of  the  domains,  the 
aggregation  operators,  the  rule-base,  and  the  consequent  analysis  are  flexible  to  the 
extent  that  they  can  be  compared  with  tuning  parameters  of  conventional  control 
architectures. 
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IV.  Fuzzy  Logic  Reactive  System 


This  research  concerns  the  development  and  validation  of  an  unconventional 
approach  to  the  problem  of  contact  force  assignment.  Conventional  methods  based 
on  global  optimization  attempt  to  satisfy  the  stated  objective  functions  while  simul¬ 
taneously  considering  all  contact  forces.  These  methods  require  numeric  optimization 
solutions  for  all  but  very  simple  contact  configurations.  This  research  will  focus  on 
an  alternative  method  which  evaluates  each  internal  force  pair  in  turn  and,  through 
fuzzy  logic  methods,  then  makes  a  global  iterative  adjustment  of  the  internal  forces. 

4.1  FLRS  Concept 

The  FLRS  algorithm  is  an  iterative  one.  Starting  with  the  known  external  force 
solution,  declare  an  error  to  exist  among  the  contact  forces  which  do  not  satisfy  their 
local  frictional  constraints.  Next,  evaluate  how  well  each  internal  force  can  reduce  the 
associated  contacts  errors  and  proportionally  weight  each  internal  force  accordingly. 
All  the  internal  forces  are  then  scaled  such  that,  when  added  to  the  previous  contact 
forces,  they  minimize  the  contact  error  for  at  least  one  contact.  The  contact  used 
to  determine  the  scale  of  the  internal  force  increment  is  the  contact  whose  error  is 
most  readily  reduced  by  the  application  of  internal  forces.  The  iterative  process  is 
continued  until  all  contacts  satisfy  the  friction  constraints.  Since  only  internal  forces 
are  being  added  to  the  external  force  solution,  the  object  wrench  due  to  the  contact 
forces  remains  the  same,  i.e.  the  addition  of  null  space  forces  to  the  pseudoinverse 
solution  cannot  change  the  object  wrench. 

4-2  FLRS  Parameters 

Figure  4.1  illustrates  how  the  FLRS  algorithm  calculates  the  internal  forces  for 
a  given  grasp  configuration  and  object  wrench.  No  knowledge  of  a  previous  solution 
is  assumed.  Before  entering  into  the  iterative  portion  of  the  algorithm,  the  external 
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forces  are  calculated  using  the  pseudoinverse  of  the  grasp  matrix.  The  first  element 
in  the  iterative  portion  of  FLRS  is  the  establishment  of  the  contact  errors  which 
are  to  be  resolved,  as  indicated  in  Figure  4.2.  The  contact  force  error  for  the 
iteration,  er_fj(t),  is  the  vector  from  the  current  contact  force,  fi(t),  to  the  closest 
point  on  the  friction  cone,  in  the  coordinate  frame. 


dxi{t) 

fix{t) 

erJi{t)  = 

dxi{t)  •  iJ,  ■  cos{(j)i{t)) 

—  < 

fM 

dxi{t)  •  //  •  sin(^,(t)) 

fUt) 

\  / 

where 


dxiif)  —  max 


fixit)  +  tJ-ifiyji)  cos(^,-(t))  + 


,0 


(4.2) 


and 

=  a  tan  2(/,>(t),  (4.3) 

The  superscript  ’ "  ’  indicates  force  variables  which  exist  in  the  iterative  portion  of 
FLRS  only.  The  x-coordinate,  in  the  frame  for  the  iteration,  of  the  closest  point 
on  the  friction  cone  to  the  current  value  of  contact  force,  is  denoted  as  dxi{t).  The 
max  operation  of  Equation  4.2  is  necessary  to  ensure  dxi{t)  >  0  even  for  ^  0. 

Given  an  initial  external  contact  force,  fexiq  and  contact  error,  er_fj(l),  if 
internal  forces  were  added  to  eliminate  the  contact  error  as  defined,  and  ignoring 
other  contacts,  the  additional  internal  force  would  result  in  the  minimum  norm 
contact  force.  The  least  internal  force  required  to  satisfy  the  friction  constraints  for 
the  given  contact,  is  er_fi(l).  However,  other  contact  errors  exist  and  their  resolution 
may  conflict  in  terms  of  the  internal  forces  necessary  to  resolve  the  contact  errors.  A 
method  of  quantifying  the  ability  of  a  given  internal  force  to  constructively  alter  the 
error  in  a  given  contact  force,  er_fi(^),  is  needed.  Figure  4.3  illustrates  the  method. 
The  internal  force  unit  vector,  from  contact  i  to  contact  j  is  denoted  e Jjj  and  is 
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Figure  4.1  FLRS  internal  force  algorithm 


Object  Surface 

Figure  4.2  Definition  of  er_fi(t) 


of  equal  magnitude  and  opposite  direction  to  eJIj,.  The  vector  dot  product  of  an 
internal  force  unit  vector  with  an  associated  error  in  contact  force,  eJ[,j  •  er_fi(f) 
and  eJlji  •er_fj(t),  are  denoted  by  er_dotij  and  er.dotji  respectively.  The  symbol 
denotes  vector  dot  product.  It  is  to  be  understood  that  er_dotij  is  a  function  of  the 
iteration,  f,  of  the  algorithm. 

The  er-dotij  scalar  quantifies  how  effectively  a  particular  internal  force  can 
resolve  an  associated  contact  force  error.  Initially,  er.dotij  E  (  _oo  oo  );  how¬ 
ever,  the  FLRS  algorithm  will  normalize  this  domain,  during  every  iteration,  such 
that  er.dotij  E  (  — 1  1  )  by  letting  ER_dot  =  ER_dot/  max(a6s(ER_dot)),  where 
ER_dot  E  denotes  the  vector  of  all  er_dotij.  Recall,  Nj  is  the  number  of 

internal  force  pairs,  (m^  —  m)/2. 

So  far  we’ve  addressed  only  the  input,  or  antecedent,  parameters  which  feed 
into  the  fuzzy  inference  portion  of  FLRS.  The  goal  of  FLRS  is  to  determine  a  solu¬ 
tion  to  the  frictional  contact  problem  by  iteratively  adding  necessary  increments  of 
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Figure  4.3  Definition  of  erjdoUj 

internal  force.  Thus,  the  output,  or  consequent,  parameter  will  be  the  relative  weight 
associated  with  a  given  internal  force,  Fuzzification  of  the  antecedents  and 

defuzzification  of  the  consequents,  using  the  method  of  Sugeno,  will  be  accomplished 
as  described  in  Chapter  III. 

4.3  FLRS  rulebase 

The  FLRS  fuzzy  inference  portion  operates  on  each  internal  force  pair,  in  turn. 
A  relative  evaluation  as  to  the  merit  of  an  increase  in  one  internal  force  pair  over 
another  must  be  made.  This  will  be  accomplished  through  fuzzy  inference  using 
a  heuristically  formed  rulebase.  The  basis  for  the  rulebase  in  evaluating  the  merit 
of  each  internal  force  is  straightforward  and  two-dimensional.  If  the  effects  of  all 
internal  forces  were  considered  simultaneously,  the  dimension  of  the  rulebase  would 
increase  to  2Ni  and  would  dramatically  increase  the  computations  necessary  for  a 
solution  as  well  as  the  complexity  of  the  rulebase.  Assuming  the  FLRS  algorithm 
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converges,  a  two-dimensional  rulebase  will  provide  an  improvement  in  solution  speed 
compared  with  the  27V/-dimensional  rulebase. 

Given  the  internal  force  and  the  associated  er.dotij  and  er^dotji,  the  outline 
of  the  rulebase  is: 

If  er_dotij  is  PositiveLarge  and  er.dotji  is  PositiveLarge  then  Wijft)  is  Large 

(4.4) 

Equation  4.4  illustrates  the  concept  that  if  erjdoUj  and  er.dotji  are  relatively  large 
positive  numbers,  then  an  increase  in  the  internal  force  associated  with  both  i  and 
j  contacts  will  greatly  decrease  the  contact  force  error  for  both  contacts.  Likewise, 
if  both  er^dotij  and  er^dotji  are  relatively  large  negative  numbers,  then  a  decrease 
in  the  internal  force  associated  with  both  i  and  j  contacts  will  greatly  decrease  the 
contact  force  error  for  both  contacts.  Also,  the  relative  magnitudes  of  the  input 
er^dot  will  dictate  the  relative  magnitude  of  the  output,  w{t).  For  instance,  if  both 
er.dotij  and  er.dotji  are  relatively  small  positive  numbers,  then  an  increase  in  the 
internal  force  associated  with  both  i  and  j  contacts  will  slightly  decrease  the  contact 
force  error  for  both  contacts. 

Figure  4.4  illustrates  the  FLRS  rulebase.  The  rulebase  is  symmetric  with 
respect  to  the  ij  and  ji  inputs.  The  diagonal  elements,  elements  for  which  the  ij 
and  ji  inputs  are  equal,  represent  the  qualitative  arguments  presented  above.  The 
off  diagonal  terms  represent  the  heuristic  notion  of  tradeoffs.  For  example,  if  erjdotij 
is  a  large  positive  number,  reflecting  the  fact  that  an  increase  in  the  internal  force 
will  greatly  decrease  the  contact  error,  and  er.dotji  is  a  relatively  small  negative 
number,  then  a  moderate  increase  in  the  internal  force  associated  with  both  i  and 
j  is  warranted  as  the  benefits  of  such  an  increase  outweigh  the  detriments.  Again, 
there  is  a  full  range  of  relative  values  for  both  the  and  contacts.  At  this  point 
some  decisions,  concerning  the  structure  of  the  fuzzy  inference  portion  of  FLRS, 
need  to  be  made. 
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Figure  4.4  FLRS  rulebase 


As  Figure  4.4  illustrates,  the  number  of  input  and  output  fuzzy  sets  has  been 
defined  to  be  seven.  The  number  of  fuzzy  sets  reflects,  to  some  degree,  the  level  of 
resolution  required  to  adequately  express  the  heuristic  rules  described  above.  Fig¬ 
ure  4.5  illustrates  the  antecedent  and  consequent  domain  and  their  associated  fuzzy 
sets.  These  sets  are  symmetric  about  zero.  The  support  of  the  antecedent  sets, 
the  interval  of  the  domain  where  the  membership  values  are  greater  than  zero,  is 
graduated  with  the  support  widening  for  the  sets  closer  to  the  domain  extremes. 
This  allows  finer  resolution  of  the  antecedents  when  they  are  relatively  small.  The 
output  domain  is  defined  for  seven  fuzzy  singleton  sets,  as  per  the  Sugeno  method 
of  defuzzification  [51].  Once  the  limits  on  the  fuzzy  domains  were  established,  the 
shape  and  mean  of  the  fuzzy  sets  were  chosen  to  resemble  examples  of  fuzzy  infer¬ 
ence  systems  common  in  the  literature,  such  as  Ross  [52].  The  Sugeno  method  of 
inference  was  chosen  to  speed  the  calculation  of  the  fuzzy  inference  consequent. 
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FLRS  Fuzzy  Sets 
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Figure  4.5  FLRS  fuzzy  antecedent  and  consequent  sets 
4.4  Change  in  Internal  Force 

After  each  internal  force  has  received  a  weight,  the  change  to  the  internal 

force  associated  with  the  contact  is  calculated  as: 

m 

(4.5) 

where  signifies  scalar  product.  The  change  in  internal  forces  are  next  scaled  and 
added  to  the  current  contact  forces  to  form  the  updated  contact  forces,  F(f). 

F(t)  =  F(t)  kit)  ■  AF{t)  (4.6) 

where 

k{t)  =  (^Af lit))*  erJ Lit)  (4.7) 
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and  L  is  the  contact  with  the  largest  value  of  Af(t),«er_fj(t).  The  symbol  denotes 
the  pseudoinverse  operator.  This  scale  factor  has  the  effect  of  reducing  er_fL(t)  in 
the  next  iteration  and  reducing  the  er_fj(t)  of  the  other  contacts  by  lesser  amounts. 
This  is  discussed  in  more  detail  in  Section  5.1.2.  The  above  definition  of  the  scale 
factor,  k{t)  promotes  the  orderly  solution  of  each  contact  force  error.  Orderly,  in 
this  case,  means  that  the  contacts  which  will  reach  the  friction  constraint  surface 
first,  due  to  the  change  in  internal  force,  will  control  the  solution  process.  As  each 
contact  force,  fj(t)  is  pushed  into  the  friction  cone  by  the  addition  of  internal  forces, 
that  contact  ceases  to  have  a  contact  force  error.  However,  during  following  iterative 
cycles,  if  necessary,  the  incremental  addition  of  internal  force  may  cause  a  previously 
zero  contact  force  error  to  become  non-zero.  The  FLRS  iteratively  cycles  until  all 
contact  forces  lie  inside  their  respective  friction  cones.  Bounds  on  k{t),  for  solution 
convergence,  will  be  established  in  Chapter  V. 

The  scale  factor  k{t)  may  be  viewed  as  a  gain,  dependent  on  the  change  in 
the  contact  internal  forces  and  error  in  force.  This  algorithm  is  analogous  to  a 
disturbance  rejection  proportional  controller  which  seeks  to  reduce  an  error  to  zero. 
Accordingly,  this  control  scheme  may  take  an  excessive  number  of  iterative  steps 
to  reach  a  zero  error  state.  Another  source  of  slowed  convergence  can  result  when 
k{t)  is  repeatedly  established  by  the  same  contact.  The  issue  of  convergence  speed 
enhancements  to  the  FLRS  algorithm,  will  be  addressed  in  Chapter  VI. 

4-5  Summary 

The  FLRS  algorithm  iteratively  weights  the  use  of  internal  forces  without  com¬ 
plete  knowledge  of  all  the  contact  error  states.  This  is  the  fundamental  difference 
between  the  FLRS  and  other  optimal  forms  of  grasp  force  assignment  which  consider 
all  error  states  simultaneously.  Both  the  FLRS  and  other  grasp  force  assignment 
methods  begin  with  some  form  of  external  force  solution,  upon  which  internal  forces 
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are  added  until  the  contact  constraints  are  met.  Useful  extensions  to  the  basic  FLRS 
algorithm  are  presented  in  Appendix  A. 
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Y.  FLRS  Convergence  Characterization 


The  grasp  force  assignment  algorithm  described  in  Chapter  IV  must  be  vali¬ 
dated.  This  chapter  will  characterize  the  limitations  on  the  parameters  governing 
the  FLRS  solution  such  that  solution  convergence  is  assured  for  both  the  two  contact 
case  and  multicontact  (m  >  2)  case.  Observations  concerning  the  characteristics  of 
the  FLRS  solution  with  respect  to  the  Nakamura  optimal  solution  will  also  be  made. 


5.1  Convergence  Analysis 

The  analysis  which  follows  will  be  planar  though  it  should  be  extendible  to 
spatial  cases.  The  objective  of  this  analysis  is  to  characterize  the  functions  and 
parameters  used  in  the  FLRS  algorithm  which  will  ensure  solution  convergence.  The 
convergence  criteria  which  will  be  satisfied  is  the  decrease,  at  each  iterative  step,  of 
the  sum  of  the  norms  of  the  error  in  contact  force  over  all  contacts. 


m 


m 


erii(t  +  l)  erJi{t) 


(5.1) 


where  t  is  an  integer  and  indicates  the  iteration  number  of  the  FLRS  algorithm.  It  is 
well  known  that  a  decreasing  and  bounded  sequence  is  convergent  [5].  As  described 
in  Chapter  IV  the  FLRS  solution  is  iterative  in  nature  and  starts  with  the  external 
force  solution, 

F(l)  =  Fext=  W^Q  (5.2) 

where  W  is  the  grasp  matrix  associated  with  the  given  grasp  configuration  and  Q 
is  the  commanded  object  wrench.. 

This  analysis  will  assume  that  the  grasp  configuration  is  consistent  with  con¬ 
cepts  regarding  force  closure,  [62].  Without  a  force  closure  grasp,  there  will  exist 
some  commanded  object  wrench  for  which  the  FLRS  algorithm  will  not  converge  to 
a  solution,  nor  would  any  other  algorithm. 
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This  analysis  will  proceed  by  first  establishing  the  contact  force  relationships 
from  one  iteration  to  the  next.  Second,  contact  force  errors  will  be  defined.  Third, 
the  square  of  the  Euclidean  norm  of  these  errors  will  be  developed  for  the  and 
{t  +  1)*^  solution  iterations,  as  in  Equation  5.1.  And  finally,  the  requirements  on 
the  contact  geometry  and  solution  parameters  will  be  derived  based  on  conformance 
with  solution  convergence.  Note,  this  convergence  analysis  uses  subscript  notation 
which  should  not  be  confused  with  standard  forms  of  tensor  notation. 

Given  a  contact  force  vector,  F(t)  at  iteration  t,  the  next  contact  force  solution 
will  be: 

F{t+l)  =  f{t)  +  kit)AF{t)  (5.3) 

where 

F(()  =  [  ff(()  fj(i)  ...  llit)  ]'’  (5.4) 

AF(i)  =  [  Aff (()  Aff(«)  ...  AfJ(i)]’'  (5.5) 

and  where  AF(t)  consists  of  internal  force  components  only  and  k{t)  is  an  overall 
scale  factor  to  be  defined.  A  diagram  of  a  three  contact  force  assignment  problem  and 
relevant  nomenclature  is  shown  in  Figure  5.1.  The  contact  coordinate  systems  are 
such  that  the  inward  pointing  normal  is  coincident  with  the  local  x-axis,  the  ^-axis  is 
perpendicular  and  out  of  the  page,  and  the  y-axis  is  consistent  with  a  right-handed 
coordinate  system. 

The  solution  for  the  {t  +  1)'*  iteration  in  contact  force  for  the  contact  is: 

%{t  +  1)  =  fi(^)  +  k{t)  ■  Afi{t)  (5.6) 

where 

m 

•  e Ju  (5.7) 

i=i 
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Figure  5.1  FLRS  solution  nomenclature  for  three  contact  problem 

and  Wij(t)  is  a  weight  associated  with  the  internal  force  between  the  and  j** 
contacts.  The  term  eJ[,j  is  the  unit  vector  from  contact  i  to  contact  j,  in  the 
coordinate  frame.  It  is  to  be  understood  that  the  summation  does  not  include  j  =  i 
as  there  is  no  Wait).  Let 

Wijif)  =  Wij{er.dotij{t)  +  erjdotji{i))  =  Wji{er^dotij{t)  +  er.dotji{t))  (5.8) 

where  Wij(t)  G  [  —  1  1  ]  is  an  increasing  function  of  er-dotij{t)  and  er-dotji{t)  which 
passes  through  the  origin,  i.e.  x  >  y  —>■  Wij{x)  >  Wij{y).  Also,  erJotij{t)  e 
[  — 1  1  ],  where 

er_dotij{t)  =  (eJij  •  er_fi(t))  fCdot  (5.9) 

and 

Cdot  =  max(eJ,j  •  erJ,(t))  (5.10) 

tj 


Object  Surface 


—  b 

Friction  Cone 


Figure  5.2  Definition  of  contact  force  zones 

As  before,  erJ‘,(t)  is  the  contact  force  error  associated  with  the  current  contact 
force,  The  contact  force  fj(t)  is  defined  to  lie  in  one  of  four  zones,  A,  B2, 
or  C,  as  depicted  in  Figure  5.2.  Each  of  these  zones  has  a  different  definition  of  the 
contact  force  error,  erjri(t).  For  the  development  which  follows,  assume  f,(t)  6  Bi, 


erjix{t)  =  -fj, 


-fM  + 

1  +  (1/2 


(5.11) 


er.Jiyit)  = 


-fiyji)  +  (ifixjt) 
1  + 


(5.12) 


We  may  now  expand  er,dotij(t): 


6T^dot-ij(ji^  —  “I”  j^do 


(5.13) 


erJot„(t)  =  (eJ-i,-,  •  +  eJ,.,,(-A(h±^))  /C,,. 


(5.14) 
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er.dotij(t)  —  /II  fJ'^-Iijx)  (5.15) 

For  this  analysis,  assume  all  contacts  have  the  same  value  for  /i,  the  static 
(Coulomb)  coefficient  of  friction.  Since  the  two  components  of  the  error  vector  are 


related  as; 


=  -H  ■  er_fiy{t) 


(5.16) 


then  we  may  simplify  the  square  of  the  norm  of  the  error, 


erJi^{tY  +  er_fiy{tf  =  (1  +  i?)er.fiy{tf 


(5.17) 


Substituting  Equation  5.12  into  Equation  5.17, 


er.fix{tf  +  er.fiy{ty  = 


{-fiy{t)  +  f^fiT{t)y 


(5.18) 


Similarly,  both  the  local  x  and  y  components  of  the  (t  +  l)  iteration  of  the  error 
in  contact  force  are: 


erjixit  +  1)  =  dxi{t  +  1)  -  fix{t  +  1) 


(5.19) 


er-M  +  I)  =  _^zM±il±^M±2) 

1  +  /W'' 


(5.20) 


_  ^  ^  ^  _  fiy{t)  +  k{t)  ■  J2T=i  Wij{t) .  eJijy  -  fi{fix{t)  +  k{t)  ■  ZT=1 

—  fi  ^  ^2 

(5.21) 

erJfiy{t  +  l)  =  iJL-  dxi{t  +  1)  -  fiy{t  +  1)  (5.22) 


erjiy{t  +  l)  =  n-  dxi{t  +  1)  -  fiy{t  +  1) 

er-M  +  1)  =  + 

1  “T  /i 


(5.23) 
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and 


er^fiy{t+l) 


Mt)  +  k{t)  •  Er=i  mj{t)  ■  eJijy  -  +  k{t)  ■  ET=i  ^iAt)  ■ 


1  +  /i^ 


(5.24) 


Using  Equation  5.17,  the  square  of  the  norm  of  the  (t  +  1)*^  iteration  of  the 
contact  force  error  is: 


er.fix(t+lA+erJiy{t+lA  =  ^ ^ 

J-  I 

(5.25) 

Equations  5.18  and  5.25,  which  describe  the  contact  force  errors  for  the  [t  +  Vf^ 
and  iteration,  can  be  substituted  into  Equation  5.1 


m  ^  ^  ^ 

J2{er-fUt  +  1)^  +  er.f,y{t  +  1)^)  -  +  erjiyitf)  <  0  (5.26) 

i=l  !=1 

or  equivalently. 


m  m 

'^[(er.fix{t  +  lf  +  er.fiy{t  +  lf)~-{erJix{tf  +  erJiy{tA)]  =  '^dni{t)  <  0  (5.27) 

i=l  t=l 

where 


fiyA'))k{t)Ylj=l'Wij[t)[fie-Iijx  C-Iijy) 
1  + 

k{ty{Y:T=i  Wi,{t){neJijx  -  eJ,jy)y 
1  + 


(5.28) 


One  could  enforce  a  more  conservative  convergence  requirement  than  Equation  5.27 
by  requiring  each  dni{t)  <  OW  i.  This  requirement  means,  for  each  contact,  that  the 
next  iteration  in  contact  force  may  not  move  away  from  the  friction  cone.  In  order  to 
assure  this,  equations  relating  all  contact  errors  must  be  formed  and  solved  simulta¬ 
neously.  Since  the  FLRS  does  not  consider  all  contacts  simultaneously,  convergence 
of  each  contact  force  at  each  step  cannot  be  expected. 
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Figure  5.3  Two  contact  problem 


5.1.1  Two  Contact  Problem.  Let’s  now  reduce  the  problem  to  m  =  2,  as 
illustrated  by  Figure  5.3.  Assuming  the  less  stringent  convergence  requirements  of 
Equation  5.27,  the  sum  of  the  difference  of  the  contact  force  norms  is: 

m  UfY  ■  w  itY 

Y^dni{t)  = - -  'I -  -  eJijyf  +  +  (5.29) 

i=\  t  + 

2k{t)  w,Yt)  (^nfi^{t)  -  ^(t))]  + 

L  IJ, 

-  eJjiy)  [nfja;{t)  -  fjy{t)) 

Forming  the  convergence  requirement  inequality  and  multiplying  all  terms  hy  1+  pY 
and  dividing  by  A:(t), 

k{t)  •  Wij{t)^  •  a  +  2wij{t)  ■  b  <0  (5.30) 

where 

a  —  e^ijy'j  T  (/^e_7j',‘a,  e—Ij{y^  (5.31) 
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and 


b  —  (^fXe-Iijx  t-Iijy)  d"  {,[^^-Ijix  ^-^jiy)  fjy{^)J  (5.32) 

Substituting  Equation  5.15  for  the  components  in  6, 

b  =  -(1  +  fi‘^)Cdot{er^dotij{t)  +  er_dotji{t))  (5.33) 


Equation  5.31  is  maximized  if  both  e-Iijy  =  —fieJijx  and  eJjiy  =  —fieJjix,  resulting 


in: 


d  —  (^‘2t[X6- — lijx^  d~ 

Also,  this  assumption  requires  eJfj^  =  and  eJj-^  — 
Equation  5.34  to: 

_  8/x^ 

Substituting  these  terms  into  Equation  5.30, 


(5.34) 

which  further  simplifies 

(5.35) 


0^2 

k{t)  ■  Wij{ty  •  +  2wij(t)  (-(1  +  fi'^)Cdot)  {erJotij{t)  +  erJotji{t))  <  0 

(5.36) 

or 

^(0  <  ^A^2~mT {^r.dotij{t)  +  erJotji{t))  (5.37) 

The  right  hand  side  of  Equation  5.37  is  always  positive  since  Wij{t)  will  always  have 
the  same  sign  as  {er .dotij{t)  +  er_dotji{t)),  from  the  definition  of  Wij{t). 


5.1.2  General  Multicontact  Problem  .  Now,  let’s  consider  the  general  mul¬ 
ticontact  case  where  m  >  2.  Assuming  the  less  stringent  convergence  requirements, 
of  Equation  5.27, 
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m  m 

0  >  ^  Y1  -  fM)Ht)  E  Wij{t){txeJij:,  -  eJijy)  +  (5.38) 

^  i=i  j=i 


7— S  KiY{J2  -  eJijy)f 

-*■  T"  M  j-=l 


We  can  form  the  equivalent  to  Equation  5.29  by  multiplying  all  terms  by  1  +  and 
dividing  by  k{t): 


2 ^ ^(/i/ia;(t)  fiy{t))wij(t)(fie^ijx  eJ[ijy)-\-k(t)^^['^Wij[t)(fieJ[ijx  e^ijy)Y  <  0 
t=i i=i  j=i  j=i 

(5.39) 

which,  after  substituting  Equation  5.15,  becomes 


-  (1  +  fi'^)Cdot2  Y,  Y  •  er.dotij{t)  +  k(t)  X)(X)  Wij{t){lie-lijx  -  e.Jijy)f  <  0 

i=l  j=l  t=l  j=l 

(5.40) 

Solving  for  the  scale  factor  k{t): 


k{t)  < 


(1  +  l^'^)Cdot2'£'^i  Wij{t)  •  er.dotij{t) 

ET=i{T.T=i  -  eJijy)y 


(5.41) 


So  far  we  have  only  considered  cases  where  f,(t)  &:  fi{t  +  1)  lie  in  region  Bi  of 
Figure  5.2;  for  fi{t)  &  f,(t  +  1)  lying  in  region  B2  the  convergence  criteria  becomes: 


i=l  j=l  i=l  j=l 

(5.42) 

Solving  for  the  scale  factor  k{t): 


k{t)  < 


(1  +  Y^)Cdot‘iTJiLi  TJi=\  •  erJotij{t) 
ET=iiET=i  +  eJijy)y 


(5.43) 


where 


er_dotij(t)  ^  fiyY'Yi^-^ijy  “b  fJ'^-Iijx)  (5.44) 
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For  the  case  of  fi(f)  &  f,(f  +  1)  lying  in  region  C,  the  contact  force  error  is: 


erXit)  = 


fix{t) 

fiy{t) 


(5.45) 


The  square  of  the  magnitude  of  the  contact  force  errors  for  the  and  (t  +  1)* 


iteration  are: 


er.fix{t)‘^  +  erJiy{tY  =  +  fiy{tf 


(5.46) 


erji^{t  +  1)^  +  erjiy{t  +  1)^  =  fi^{t  +  1)^  +  fiy{t  +  1)^ 

m 

=  ifixit)  +  k{t)Y^Wij{t)eJij:^y  + 

3=1 

m 

ifiyit)  +  Kt)  Y1  Wij{t)eJijyf 
i=i 

The  convergence  criteria  equation,  Equation  5.26,  becomes: 


(5.47) 


m 

E  (/-(')" + hm)  <  0 


(5.48) 


which  can  be  reformed  into. 


Kt)  S  (51  Wij(t)eJij^Y  +  Wij{t)eJijyY  < 

i=i  \  i=i  j=i  / 


■2E  E 

i=l  \j=l  j=l 


(5.49) 
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with  which  we  may  deduce 


k{t)  < 


-2 »«-!.>) 

Er=.  {{Z%i  +  (E"=, 


(5.50) 


Of  course,  in  general,  any  iteration  solution  will  have  contact  forces  lying  in  all 
four  regions.  Assuming  f,(i)  &  fj(t  +  1)  lie  in  the  same  region,  for  any  i,  and  the 
contacts  lying  in  each  region  may  be  enumerated  as; 


1  ^  niA 

f,(t)  G  A 

rriA  +  l  ^  rriBi 

m  e  B 

rriBi  +  1  mB2 

m  6  B: 

mB2  +  1  ^  m 

l{t)  €  C 

(5.51) 


then 


(1  +  Z)i=m^+l  I^J=1  ■  ^T.dotij[t) 

(1  +  ZU  •  erJotM 

Er=i^,+i(Er=i  + ej,jy)y 

-2E£^,,+i  Ef^i  •  (f,(0  >  eJ,,) 

z:r=„,.+,  ((£”.,  wii(t)ej.i.r + c;"=. 


Thus,  we  have  established  an  upper  limit  on  the  overall  scale  factor  k{t).  The 
method  described  in  Chapter  IV  to  calculate  k{t)  must  be  checked  to  ensure  it  is  less 
than  k{t)  as  calculated  through  Equation  5.52;  alternatively,  and  more  practically, 
one  could  monitor  the  YL1L\  dni{t)  to  ensure  the  sum  is  less  than  zero  and  only  use 
Equation  5.52  to  develop  a  suitable  k{t)  when  the  convergence  check  fails.  Next,  we 
need  to  establish  a  lower  bound  on  k{t). 

In  Chapter  IV,  k{t)  was  calculated  as: 


k{t)  =  Aff  (t)er_fi:,(t). 


(5.53) 
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<>s.  A 

Figure  5.4  Relationship  between  k{t),  er_f£,(t),  and  Afj[,(t) 


where  L  is  the  contact  which  has  the  greatest  value  of  Af  (t)#  er_f  (t).  The  superscript 
#  indicates  the  pseudoinverse  operator,  A*  =  (A^A)“^  A^.  Since  Af£,(t)  is  a  vector, 
Equation  5.53  may  be  expanded  to: 


k{t)  =  (AfJ(t)eriL(t))/(AfJ(t)AfL(t)).  (5.54) 

For  Afi,(t)  •  er_fi;,(t)  >  0  k{t)  >  0  while  for  Afi(t)  •  er_fi(t)  <  0  k{t)  <  0. 
This  overall  scale  factor  provides  a  mechanism  to  ensure  an  appropriate  magnitude 
of  change  for  the  contact  forces.  Assuming  that  the  contact  is  known,  k{t)  scales 
the  change  in  contact  force  for  the  contact  such  that  the  norm  of  the  difference 
between  k{t)  ■  and  erTi:,(t)  is  minimized.  A  graphical  representation  of  this 

solution  is  illustrated  in  Figure  5.4. 


5.1.S  FLRS  and  the  Optimal  Solution.  Both  the  FLRS  algorithm  and  the 
optimal  solution  proposed  by  Nakamura  begin  with  the  external  force  solution,  which 
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is  itself  the  minimum  norm  of  the  contact  force  solution,  without  constraints.  The 
FLRS  seeks  solutions  whereby  internal  forces  are  added  to  any  contact  forces  which 
violate  the  friction  constraints.  Ideally,  for  a  given  contact,  FLRS  adds  internal  forces 
which  are  coincident  with  a  vector  from  the  external  force  to  the  nearest  point  in 
force  space  which  satisfies  the  friction  constraints,  i.e.  perpendicular  to  the  nearest 
point  on  the  surface  of  the  friction  cone  in  Figure  5.4. 

By  defining  er_f,(f)  as  the  shortest  path  to  the  constraint  surface,  and  using 
the  fuzzy  control  surface  described  in  Chapter  IV  to  calculate  we  are  seeking 

a  solution  in  which  we  attempt  to  use  as  little  internal  force  as  necessary  to  satisfy 
the  frictional  constraints.  This  is  qualitatively  the  same  goal  the  optimal  solution 
seeks;  the  minimum  internal  force  necessary  to  satisfy  the  frictional  constraints.  The 
ability  of  the  FLRS  to  accomplish  this  goal  is  hampered  by  the  fact  that  the  solution 
is  accomplished  considering  contacts  pairwise  while  the  optimal  solution  considers 
all  contacts  together.  Thus,  in  general,  one  should  expect  the  FLRS  solution  to  be 
suboptimal  with  respect  to  the  solution  proposed  by  Nakamura. 

We  could  change  the  character  of  the  FLRS  solution  by  altering  the  definition 
of  er_fj(i)  and/or  the  function  Appendix  A  documents  how  redefining  er_fj(t) 

and  the  FLRS  exit  criteria  can  enforce  a  given  level  of  contact  stability  in  the  face 
of  contact  force  disturbances.  By  changing  the  definition  of  we  can  further 

alter  the  character  of  the  solution,  this  is  discussed  in  Appendix  A. 

5.2  Summary 

This  chapter  has  developed  limits  on  the  FLRS  parameters  in  order  for  the 
system  to  converge  to  a  solution  given  a  force  closure  grasp  configuration  and  object 
wrench.  The  internal  force  weighting  function,  has  been  limited  to  one  of  an 

increasing  function  of  er.dotij  and  erjdotji.  The  scale  factor,  k{t),  for  the  change  in 
internal  forces  for  the  iteration  has  an  upper  bound  according  to  Equation  5.52 
and  a  lower  bound  of  zero.  The  nominal  scale  factor  definition,  from  Chapter  IV,  has 
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a  specific  geometric  relationship  between  and  Afh(t).  The  FLRS  solution 

is  suboptimal,  with  respect  to  the  solution  proposed  by  Nakamura,  and  it  may  be 
altered  by  changing  the  rules/functions  which  govern  its  behavior. 
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VI.  FLRS  Implementation  Refinements 

The  FLRS  algorithm  defined  in  Chapter  IV  and  analyzed  in  Chapter  V  may  be 
considered  ideal.  Since  the  FLRS  solution  may  be  characterized  as  a  position  control 
scheme,  though  the  solution  will  converge,  it  may  take  an  excessive  number  of  itera¬ 
tions  to  do  so.  The  initial  implementation  of  the  FLRS  algorithm  was  accomplished 
using  the  MATLAB  computational  environment.  During  the  implementation  of  the 
FLRS  algorithm,  two  refinements  were  made  related  to  solution  acceleration.  The 
first  change  concerned  the  redefinition  of  erJf,(t).  The  second  refinement  was  the 
development  of  an  alternative  method,  relative  to  the  definition  given  in  Chapter  IV, 
to  determine  the  internal  force  scale  factor,  k[t). 


6. 1  Redefinition  of  Contact  Force  Errors 

One  absolute  requirement  of  the  FLRS  algorithm  is  that  no  contact  force  may 
violate  the  friction  constraints.  Thus,  we  require  a  zero  error  solution  using  a  method 
which  may  converge  slowly  near  the  constraint  surface  due  to  the  position  control 
characteristics  of  the  solution.  In  order  to  speed  the  convergence  of  the  FLRS  so¬ 
lution,  er_f,(^)  is  redefined  to  promote  faster  convergence.  The  new  definition  of 
er_fi(t)  will  be  equivalent  to  shifting  the  friction  cone  of  the  z**  contact  inward  along 
the  local  Xi  axis  by  a  distance  8x  and  will  be  called  the  convergence  cone,  as  illus¬ 
trated  in  Figure  6.1.  The  distance  8x  will  be  referred  to  as  the  convergence  cone 
offset. 

Referring  to  Figure  6.1,  the  new  definition  of  the  contact  force  error  where 
fi(t)  6  Bi  will  be: 


er_f,(t)  = 


dxfit) 

//  •  (dxfit)  —  8x) 


fix{t) 


(6.1) 
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yt 


Figure  6.1  Definition  of  convergence  cone 


where 

dxi{t) 

For  contact  forces  fj(t)  G  B2, 


fixjt)  + 

1  + 


(6.2) 


er  j((t) 


dxi{t) 

—fx  ■  {dxi{t)  —  6x) 


(6.3) 


where 


and  for  fi(t)  6  C, 


dxi{t) 


fixid^  M  ■  fiyij'')  “f  dx 

1+ 


(6.4) 

(6.5) 


Referring  to  Figure  4.1,  the  friction  constraint  satisfaction  check  made  at  the  end 
of  every  iteration  is  still  based  on  the  friction  cone,  not  the  convergence  cone.  The 
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Figure  6.2  FLRS  solution  steps  for  two  contact  problem 


effect  of  this  change  in  the  FLRS  algorithm  may  be  demonstrated  using  the  two 
contact  example  of  Chapter  II. 

Recall  the  problem  and  solution  to  the  two  contact  example  solved  using  the 
optimal  method  described  by  Nakamura: 


Now  examine  the  solution  using  the  original  FLRS  algorithm  without  the  new  def¬ 
inition  for  er_fi(t).  Figure  6.2  illustrates  the  iterative  development  of  the  contact 
force  components  for  both  Contacts  1  and  2.  Even  after  30  iterations  of  the  FLRS 
algorithm,  the  friction  constraints  have  not  been  satisfied.  It  should  be  clear  that 
the  contact  forces  will  not  satisfy  the  constraints  in  a  finite  number  of  iterations. 
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Figure  6.3  FLRS  solution  steps  with  convergence  cone 


Figure  6.3  illustrate  the  FLRS  solution  using  the  redefined  er_fj(t),  where  6x  =  0.05. 
These  solutions  satisfy  the  frictional  constraints  after  24  of  iterations  of  the  FLRS 
algorithm.  Solution  sensitivity  to  the  convergence  cone  offset  will  be  addressed  later. 


6.2  Refined  Scale  Factor 

Another  situation  leading  to  slowed  solution  convergence  is  the  case  where  a 
single  contact  L,  is  repeatedly  controlling  solution  convergence  through  the  calcula¬ 
tion  of  k{t),  as  defined  by  Equation  5.54.  This  situation  typically  occurs  when  all  but 
one  or  two  contact  forces  satisfy  the  friction  constraints,  and  Afz;,(t)  •  erJL(t)  <C  1. 
Referring  to  Figure  5.4,  this  situation  arises  when  Af£,(t)  •  er_fi(t)  <C  1,  or  Afi,(t) 
is  nearly  perpendicular  to  er_f£,(t).  Thus,  fc(t)Afi(t)  will  be  small  and  the  solution 
will  slowly  converge.  For  these  cases,  a  refined  definition  for  k{t)  will  allow  a  speedy 
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Figure  6.4  FLRS  solution  steps  with  convergence  cone  and  k 


solution.  This  situation  may  be  demonstrated  using  the  two  contact  example  of 
Chapter  II. 

Using  the  redefined  erjfi(t),  where  8x  =  0.05,  the  FLRS  algorithm  exhibited 
the  slow  convergence  behavior  seen  in  Figure  6.3.  Recognizing  the  repetitive  nature 
of  the  solution,  one  could  extrapolate  a  previous  solution  to  determine  a  better  value 
for  the  scale  factor  k{t).  Assume  the  FLRS  algorithm  has  just  calculated  k{t)  using 
the  same  contact  L  for  the  past  n  iterations,  where  n  is  the  iteration  threshold  above 
which  FLRS  will  use  an  alternative  method  to  calculate  k{t).  Instead  of  using  k{t), 
we  will  extrapolate  to  determine  a  value  k{t)  which  will  cause  er_fjr,(t  +  1)  =0. 
Let, 

er_f£,(t)  ^ 

Af£,(t)  •  erJL(t) 

Figure  6.4  illustrates  the  change  in  FLRS  solution  behavior  with  the  refined  scale 
factor  definition  included.  The  solution  converged  in  just  five  iterations. 


k{t) 
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These  two  refinements  to  the  FLRS  algorithm  allow,  what  is  essentially  a 
proportion  control  law,  to  converge  relatively  quickly.  Chapter  VII  explores  numeric 
examples  of  FLRS  solution  sensitivity  to  changing  8x  and  Section  A. 4  discusses 
other  means  by  which  the  FLRS  convergence  properties  may  be  improved. 
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VIL  FLRS  Numeric  Examples 


Three  grasp  configurations  were  used  to  numerically  validate  the  FLRS  solution 
method.  The  FLRS  algorithm  is  now  assumed  to  include  both  the  convergence  cone 
and  k  amendments  discussed  in  Chapter  VI.  The  three  configurations  consist  of 
three,  four,  and  five  contacts  on  a  unit  radius  sphere.  Each  configuration  was  tested 
with  72  coplanar  commanded  object  wrenches.  Each  solution  was  accomplished  using 
the  FLRS  algorithm,  the  numerical  equivalent  to  the  Nakamura  optimal  solution, 
and  a  realistic  approximation  to  the  Nakamura  optimal  solution.  The  three  contact 
case  was  also  solved  using  the  method  proposed  by  Yoshikawa.  Also,  the  five  contact 
configuration  was  tested  with  72  random  spatial  commanded  object  wrenches.  The 
MATLAB  code  used  to  generate  the  FLRS  solutions  is  included  in  Appendix  B. 

7.1  Grasp  configurations 

The  five  contacts  used  for  the  numeric  examples  are  illustrated  in  Figure  7.1. 
The  column  of  the  matrix  Pos  is  the  position  vector  of  the  contact  in  the 
object  frame. 

10-10  0 
Pos  =  0  -1  0  0.707  0.707 

0  0  0  0.707  -0.707 

The  three,  four,  and  five  contact  configurations  applied  contacts  {1, 2, 3},  {1, 2,  3, 4}, 
and  {1,2, 3, 4, 5}  respectively.  The  commanded  object  wrenches,  for  the  first  series 
of  examples,  coincides  with  the  object  {x,  y)  plane. 

Q  =  cos(0)  sin(^)  0  0  0  0  (7-2) 

where  d  =  z  *  7r/36,  and  has  units  of  degrees,  and  i  =  1  :  72.  Contacts  {1,  2, 3}  lie  in 
the  plane  of  the  commanded  object  wrenches. 
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Contact  5 


Contact  2 

Figure  7.1  Example  object  and  five  contacts 


1.2  Solution  methods 

All  numeric  examples  were  solved  using  three  methods.  The  first  method 
used  the  FLRS  algorithm.  The  second  consisted  of  a  numeric  approximation  to 
the  Nakamura  optimal  solution.  The  numeric  approximation  was  accomplished  us¬ 
ing  the  MATLAB  constrained  optimization  function  CONSTR  [1,  19].  This  algo¬ 
rithm  solves  the  Kuhn-Tucker  equations,  Equation  2.38  and  2.39,  through  the  use 
of  constrained  quasi-Newton  methods  which  guarantee  superlinear  convergence  by 
accumulating  second  order  information  regarding  the  Kuhn-Tucker  equations  using 
a  quasi-Newton  updating  procedure.  These  methods  are  commonly  referred  to  as 
sequential  quadratic  programming  (SQP)  methods.  A  quadratic  programming  sub¬ 
problem  is  solved  at  each  major  iteration  which  is  then  used  to  generate  the  search 
direction  for  a  line  search  procedure.  The  CONSTR  function  may  be  given  analytic 
gradients  for  the  objective  function  and  constraint  functions,  or  the  gradients  may 
be  numerically  approximated. 
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The  CONSTR  function  allows  the  user  to  set  the  acceptable  error  bounds  on 
the  independent  variable,  the  constraint  functions,  and  the  objective  function.  For 
this  case,  the  independent  variable  was  the  vector  y,  the  constraint  functions,  g(y), 
were  those  of  Equation  2.34,  and  the  objective  function,  h(y),  was  F^F.  For  the 
numeric  approximation  to  the  optimal  solution,  the  error  bounds  for  these  elements 
were  set  very  small,  Zy  =  0.001,  Zg  =  0.00001,  Zh  =  0.001.  However,  even  with  the 
error  bounds  set  very  small,  many  of  the  solutions  obtained  through  the  CONSTR 
function  actually  violate  the  frictional  constraints.  For  the  purposes  of  these  exam¬ 
ples,  we  will  ignore  this  fact.  The  figures  below  present  this  CONSTR  solution  as 
the  theoretical  Nakamura  solution. 

A  practical  analog  to  the  above  optimal  solution  was  used  to  make  relevant 
comparisons  between  the  method  proposed  by  Nakamura  and  the  FLRS  method. 
The  realistic  analog  uses  a  displaced  friction  cone,  similar  to  the  one  used  for  the 
FLRS  convergence  cone.  Since  errors  exist  in  the  numeric  solution  and  the  frictional 
constraints  cannot  be  violated,  the  nominal  frictional  constraints  used  in  the  CON¬ 
STR  function  must  be  displaced  inward  from  the  actual  constraints.  This  allows  all 
solutions  within  the  given  error  bounds  to  fall  inside  the  actual  constraint  boundary. 
In  an  effort  to  achieve  similar  solutions  between  the  realistic  optimal  method  and  the 
FLRS  method,  the  nominal  friction  constraints  were  equivalent  to  the  convergence 
cone  of  the  FLRS  solution.  The  friction  constraints  were  shifted  inward  6x  along  the 
local  x-axes. 

With  the  nominal  frictional  constraints  set,  the  error  bounds  on  the  indepen¬ 
dent  variables  and  the  objective  function  were  relaxed  to:  Zy  =  0.1  and  Zh  =  0.1. 
The  error  bound  on  the  constraint  functions,  e^,  was  increased  until  solutions  for  F, 
began  to  exceed  the  original  friction  constraint  boundary.  The  same  error  bound  was 
used  for  all  examples,  Zg  =  0.027.  Thus,  the  practical  Nakamura  solution  achieved 
admissible  results,  nominally  the  same  results  as  the  FLRS  algorithm,  while  reducing 
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the  number  of  iterations  necessary  to  converge,  by  relaxing  the  error  bounds.  This 
solution  will  be  referred  to  as  the  practical  Nakamura  solution. 

The  three  contact  problem  is  also  solved  using  the  method  proposed  by  Yoshikawa. 
This  method  also  uses  the  CONSTR  optimization  routine  to  solve  for  internal  forces. 

As  with  the  Nakamura  solution,  the  Yoshikawa  solution  will  have  both  a  theoretical 
and  practical  solution.  The  optimization  related  error  bounds  on  the  Yoshikawa 
solutions  will  be  the  same  as  those  for  the  Nakamura  solutions  for  the  sake  of  com¬ 
parison. 

As  mentioned  above,  the  CONSTR  algorithm  is  capable  of  using  analytic  gra¬ 
dient  input  for  both  the  objective  function  and  constraint  functions,  with  respect 
to  the  unknown  variable  y.  The  CONSTR  solution  data  included  in  this  document 
were  obtained  using  the  default  numeric  approximation  for  the  objective  and  con¬ 
straint  function  gradients.  Results  obtained  using  analytic  gradient  information  were 
not  significantly  different  in  terms  of  contact  force  solution  or  the  number  of  FPOs 
required  for  the  solution. 

7.5  Planar  Object  Wrench  Study 

Figures  7.2,  7.4,  and  7.6  illustrate  how  well  the  practical  Nakamura  and  FLRS 
solutions  followed  the  theoretical  Nakamura  solution.  Both  the  practical  Nakamura 
and  the  FLRS  solutions  appear  to  be  close  to  the  theoretical  Nakamura  solution  such 
that  one  would  conclude  they  approximate  a  least  squares  solution.  Figure  7.2  also 
illustrates  the  Yoshikawa  solution;  for  the  most  part,  the  theoretical  solution  lies  on 
theoretical  Nakamura  solution.  One  area  where  there  is  some  noticeable  deviation  is 
near  9  =  90°.  This  is  a  manifestation  of  the  Yoshikawa  definition  of  manipulating  and 
grasping  force  directions  which  can  generate  asymmetric  grasping  and  manipulative 
force  directions  for  a  symmetric  problem. 

The  Euclidean  norm  of  the  contact  forces  is  presented  rather  than  all  the 
components  of  all  the  contact  forces,  which  would  appear  as  almost  identical  plots 
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Norm  of  Applied  Contact  Force 


Figure  7.2  Norm(F)  for  three  contact  problem 


between  the  three  solution  methods.  The  plot  of  normiY)  actually  exaggerates  dif¬ 
ferences  between  the  solutions.  Figures  7.3,  7.5,  and  7.7  illustrate  the  number  of 
floating  point  operations  (FPOs)  required  to  achieve  admissible  solutions  for  both 
the  practical  Nakamura  algorithm  and  the  FLRS  algorithm.  The  FPO  count  began 
just  before  external  force  calculation  was  performed  until  an  admissible  contact  force 
solution  was  accomplished,  for  all  methods.  The  pseudoinverse  of  the  grasp  matrix  is 
required  only  once  for  a  given  grasp  configuration  for  the  methods  illustrated,  except 
the  Yoshikawa  solution  which  doesn’t  use  one.  The  plots  illustrate  the  dramatic  de¬ 
crease  in  required  FPOs  for  the  FLRS  solution  compared  to  the  practical  Nakamura 
solution. 

Figure  7.8  illustrates  the  root  mean  square  (RMS)  of  the  FPOs  for  the  72 
solutions  for  the  three,  four,  and  five  contact  problems.  The  results  for  the  practical 
Nakamura  case  show  the  dramatic  increase  in  required  FPOs  with  an  increase  in  the 
number  of  contacts.  The  FLRS  case  illustrates  relative  independence  between  the 
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Figure  7.7  FPOs  for  five  contact  problem 

required  FPOs  and  the  number  of  contacts.  The  Yoshikawa  data  point  illustrates 
the  degree  to  which  his  proposed  solution  accelerated  the  internal  force  solution 
process.  The  increase  associated  with  the  FLRS  solution  of  the  four  contact  problem 
is  due,  in  part,  to  the  asymmetry  of  the  particular  grasp  configuration,  relative  to  the 
commanded  object  wrenches.  The  ratio  of  RMS  FPOs  for  the  practical  Nakamura 
solutions  to  the  FLRS  solutions  are:  4.6,  19.7,  and  81.3  for  the  three,  four,  and  five 
contact  problems  respectively. 

7-4  Random  Object  Wrench  Study 

The  following  numeric  example  used  the  five  contact  configuration  illustrated 
in  Figure  7.1.  However,  the  commanded  object  wrenches  consisted  of  72  random 
spatial  vectors  where  each  element  of  the  wrench  was  bounded,  Qi  E  [  —I  1  ]. 
Figure  7.9  illustrates  the  norm(F)  results  for  the  commanded  wrenches.  The  data 
was  sorted  such  that  the  norm(F)  for  the  theoretical  Nakamura  solution  increases 
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RMS  FPOs 


Figure  7.8  RMS  FPOs  for  the  three,  four,  and  five  contact  problems 

from  the  first  commanded  wrench  to  the  last.  This  was  accomplished  to  order  to 
present  coherent  results  rather  than  noise.  Figure  7.10  presents  the  FPO  data  which 
again  shows  the  superiority  of  the  FLRS  algorithm  compared  with  the  practical 
Nakamura  solution,  the  RMS  FPO  ratio  was  49.8.  The  FLRS  algorithm  did  not  fail 
to  converge  to  any  of  the  random  object  wrenches. 

The  illustrated  results  of  the  numeric  examples  clearly  show  the  tremendous 
advantage  in  terms  of  FPOs  of  the  FLRS  algorithm  over  the  practical  Nakamura 
solution.  The  norm(F)  plots  show  that  both  algorithms  obtain  contact  force  solu¬ 
tions  close  to  the  theoretical  Nakamura  solution.  The  advantage  in  FPOs  translates 
into  a  significant  computational  advantage  for  real-time  applications  using  the  FLRS 
method  as  will  be  quantified  in  Section  7.7  which  discusses  a  real-time  implementa¬ 
tion  of  the  FLRS  algorithm  and  the  results. 
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Figure  7.9 


Norm(F)  for  the  five  contact  problem  with  sorted  random  Q 
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Figure  7.10  FPOs  for  five  contact  problem  with  sorted  random  Q 


7.5  FLRS  Convergence  Cone  offset  Sensitivity 

As  mentioned  earlier,  the  convergence  cone  offset,  for  the  numeric  examples 
illustrated,  has  been  8x  =  0.05.  The  fact  that  the  convergence  cone  is  used  requires 
the  contact  force  solutions,  determined  through  the  FLRS  algorithm,  to  have  greater 
levels  of  contact  force  relative  to  the  theoretical  minimum  norm  solution.  This  is 
apparent  in  all  the  previous  figures.  Clearly,  larger  convergence  cone  offsets  will 
cause  the  FLRS  solutions  to  have  larger  normed  contact  forces.  Conversely,  smaller 
convergence  cone  offsets  will  require  a  greater  number  of  solution  steps  in  order  to 
converge  to  a  solution.  Clearly,  a  tradeoff  exists  between  solution  accuracy  (based 
on  the  theoretical  solution)  and  solution  speed. 

Figures  7.11  and  7.12  illustrate  this  tradeoff  for  the  three  contact  example  of 
Section  7.3.  The  ordinate  of  Figure  7.11  refers  to  the  RMS  of  the  difference  between 
the  norm  of  the  contact  forces  calculated  by  the  FLRS  algorithm  and  the  theoretical 
Nakamura  method  for  72  planar  input  wrenches.  The  input  wrenches  are  the  same 
as  those  used  for  the  previous  three  contact  example.  Equation  7.2.  Figure  7.12 
illustrates  the  change  in  the  RMS  of  the  floating  point  operations  required  for  each 
solution  for  progressively  larger  values  of  Sx.  The  significant  aspect  of  these  figures 
is  the  knee  of  the  curves,  which  is  the  basis  for  the  decision  to  use  8x  =  0.05  as  the 
nominal  setting  throughout  this  paper. 

7.6  FLRS  Coefficient  of  Friction  Sensitivity 

Up  to  this  point,  all  examples  have  used  a  coefficient  of  friction,  n  =  0.4. 
Figure  7.13  illustrates  the  difference  is  contact  force  solutions  for  several  values  of 
fi.  The  three  contact  example  of  Section  7.3  is  used  for  this  illustration.  Figure  7.14 
illustrates  a  decrease  in  number  of  FPOs  with  an  increase  in  (i.  The  RMS  FPOs  are 
6160,  4127,  and  3178  for  /j,  =  0.4,  0.5,  and  0.6  respectively. 
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Figure  7.11  RMS  Error  for  three  contact  problem 


RMS  FPOs  versus  Offset 


Figure  7.12  RMS  FPOs  for  three  contact  problem 
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Figure  7.13  Norm(F)  for  three  contact  problem 


Figure  7.14  FPOs  for  three  contact  problem 
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7.7  Real-Time  FLRS 


The  final  step  of  the  validation  process  involved  a  real-time  evaluation  of  the 
FLRS  algorithm.  The  FLRS  algorithm  was  converted  from  the  MATLAB  interpre¬ 
tive  environment  to  a  common  form  of  the  C  language.  Gnu  C  is  a  common  Unix 
based  C  compiler,  and  one  which  supports  the  Chimera  real-time  operating  system. 
The  Chimera  3.1  real-time  operating  system  which  was  used  for  the  validation  was 
developed  at  Carnegie  Mellon  University  (CMU). 

Chimera  resides  on  a  host  computer,  which  in  this  case  is  a  Sun  SPARCsta- 
tion2,  and  provides  control  over  at  least  one  single  board  computer,  or  real-time 
processing  unit  (RTPU),  installed  in  a  VMEbus  system.  Chimera  allows  properly 
written  and  compiled  modules,  similar  to  conventional  functions,  to  be  downloaded 
from  the  host  machine  to  the  RTPU  and  executed.  Chimera  provides  the  neces¬ 
sary  overhead  to  synchronize  the  execution  of  the  modules  and  to  exchange  data 
between  them  through  a  common  global  state  variable  table.  The  RTPU  used  for 
the  validation  was  a  Motorola  68030  microprocessor. 

The  converted  FLRS  code  was  completed  as  a  single  Chimera  module  and  is 
included  in  Appendix  C.  In  order  to  test  the  module,  an  input /output  module  was 
written  to  provide  the  input  data;  grasp  configuration,  external  force  solution,  and 
start  time;  and  record  the  output  data;  contact  forces  and  end  time.  The  output 
contact  forces  were  used  to  verify  proper  module  execution  while  the  difference  in 
start  and  end  times  were  used  to  characterize  the  real-time  capability  of  the  FLRS 
algorithm.  The  real-time  test  involved  the  three  contact  problem  of  Section  7.3.  The 
72  planar  wrenches  were  consecutively  input  to  the  FLRS  module  and  the  results 
were  recorded.  Figure  7.15  illustrates  the  time  required  for  the  contact  force  solution 
for  each  of  the  input  wrenches.  The  maximum  time  required  for  a  solution  was 
58  milliseconds.  This  is  fast,  though  improvements  in  speed  may  be  accomplished 
by  optimizing  the  FLRS  code  and  taking  advantage  of  the  parallel  aspects  of  the 
algorithm.  Since  the  FLRS  algorithm  evaluates  the  internal  force  weight,  Wij{t) 
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Figure  7.15  FLRS  execution  time  for  the  three  contact  problem 

independently  for  each  internal  force  pair,  a  parallel  implementation  of  the  FLRS 
may  reduce  the  execution  time  markedly. 

7.5  Summary 

The  numeric  examples  illustrated  the  dramatic  decrease  in  solution  FPOs  of 
the  FLRS  algorithm  relative  to  the  practical  Nakamura  method.  The  FLRS  was  also 
shown  to  achieve  results  very  close  to  the  optimal  solution  formulated  by  Nakamura. 
Finally,  the  FLRS  algorithm  was  shown  to  be  fast  when  implemented  on  real-time 
hardware. 
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VIII.  Conclusion 


This  research  explores  a  new  method  for  calculating  the  contact  forces  required 
to  stably  manipulate  a  grasped  object  with  frictional  point  contacts.  Many  authors  in 
the  field  of  robotic  grasping  have  conducted  extensive  research  in  this  area.  One  long 
standing  problem  in  this  area  has  been  the  determination  of  the  static  forces  required 
to  resist  a  given  object  wrench  with  a  known  contact  configuration.  A  consistent 
theme  throughout  the  published  works  is  the  need  for  a  fast,  reasonable,  and  flexible 
method  to  determine  the  internal  forces  for  such  a  problem.  The  difficulty  arises 
from  the  fact  that,  in  general,  an  infinite  number  of  valid  solutions  exist.  Various 
definitions  of  optimal  solutions  have  been  proposed  to  limit  the  solution  space.  This 
research  used  the  optimal  definitions  associated  with  Nakamura  et  al.  His  work 
has  emphasized  grasp  force  solutions  which  minimize  the  Euclidean  norm  of  all  the 
contact  forces  applied  to  the  grasped  object. 

The  goal  of  this  research  was  to  develop  a  fast,  reasonable,  and  flexible  method 
to  determine  the  internal  forces  for  a  given  grasp  configuration  and  object  wrench. 
Fast,  means  an  algorithm  capable  of  real  time  results  required  for  successful  robot 
grasping.  Reasonable  in  this  case  refers  to  solutions  qualitatively  similar  to  the 
minimum  Euclidean  norm  solutions  proposed  by  Nakamura.  Flexible  is  with  regard 
to  algorithm  changes.  One  can  easily  change  the  character  of  the  solution  while  still 
using  the  same  fundamental  algorithm.  The  algorithm  which  was  developed,  FLRS, 
accomplishes  these  tasks. 

The  FLRS  algorithm  is  based  on  a  tunable  fuzzy  logic  inference  method  which 
can  be  altered  to  achieve  changes  in  the  character  of  the  FLRS  solutions.  The 
rulebase  used  in  this  paper  has  emphasized  solutions  qualitatively  similar  to  the 
minimum  norm  solutions  of  Nakamura.  FLRS  does  not  seek  to  simultaneously  resolve 
appropriate  internal  force  levels  for  a  given  contact  force  error  state,  as  do  published 
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optimization  methods.  Instead,  FLRS  weights  changes  in  the  internal  forces  on  a 
pairwise  basis.  This  results  in  a  comparatively  fast  algorithm. 

Based  on  a  comparison  with  the  Nakamura  solution  method,  FLRS  is  signifi¬ 
cantly  faster  for  a  three  contact  problem  and  is  dramatically  faster  for  the  four  and 
five  contact  problems  explored.  The  solutions  are  very  close  to  the  theoretical  min¬ 
imum  norm  solutions.  In  order  to  verify  the  real  time  capability,  the  three  contact 
problem  was  accomplished  using  a  compiled  version  of  FLRS  uploaded  to  a  Motorola 
68030  processor.  The  real  time  results  were  also  good,  with  a  mean  of  24ms  for  each 
solution  and  a  maximum  of  58ms. 

8. 1  Contributions 

This  research  has  made  a  number  of  important  contributions  to  the  field: 

•  Established  a  floating  point  operation  baseline  of  Nakamura’s  algorithm 

•  Differentiated  between  ’theoretical’  and  ’practical’  problem  formulations  for 
optimal  solutions 

•  The  algorithm  of  Yoshikawa  was  shown  to  be  suboptimal  with  respect  to 
norm(F) 

•  The  FLRS  algorithm  was  established 

—  Defined  the  contact  force  error 

-  Evaluated  the  internal  forces  on  a  pairwise  basis 

-  Used  a  least  squares  internal  force  scale  factor 

—  Established  an  upper  bounds  on  the  scale  factor  to  ensure  solution  con¬ 
vergence 

—  Enhanced  the  convergence  speed  through  the  definition  of  a  convergence 
cone  and  through  scale  factor  extrapolation 
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•  FLRS  has  dramatically  decreased  solution  floating  point  operations  relative  to 

Nakamura’s  solution 

•  FLRS  solution  mimics  the  minimum  norm  solution  proposed  by  Nakamura 

•  FLRS  algorithm  robustly  solved  problem  of  random  spatial  object  wrenches 

8.2  Recommendations 

The  FLRS  algorithm  is  poised  to  enable  strides  in  both  simulation  and  hard¬ 
ware  implementations  of  stable  grasping  algorithms.  Currently,  work  is  proceeding 
at  AFIT  which  can  use  the  FLRS  algorithm  as  an  integral  part  of  the  object  resolved 
telerobotic  (ORT)  architecture  for  multifingered  grasps.  This  work  is  partially  an 
extension  of  the  force  control  concepts  of  Nakamura.  The  ORT  requires  an  algo¬ 
rithm  which  can  calculate  contact  forces  required  to  resist  grasped  object  wrenches 
and  provide  contact  stability.  The  FLRS  algorithm,  documented  here,  can  fill  this 
need. 

One  of  the  advantageous  qualities  of  the  FLRS  algorithm  is  the  ability  to  easily 
take  advantage  of  parallel  computer  architecture.  The  FLRS  algorithm  operates  on 
the  contacts  in  an  independent  pairwise  basis;  this  independence  leads  to  a  rela¬ 
tively  easy  implementation  of  simultaneous  computation  on  parallel  processors.  The 
MATLAB  and  C  code  developed  to  date  has  not  taken  advantage  of  this  possibility. 
Decreased  execution  time  would  be  the  incentive  for  such  a  change. 

This  research  has  not  included  significant  exploration  of  the  rulebase  space  and 
possible  solution  enhancements  with  changes  in  the  rulebase.  Typical  methods  for 
such  exploration  use  automatic  genetic  algorithms  seeking  extremes  of  a  given  objec¬ 
tive  function.  Also,  the  functional  dependence  of  the  convergence  offset  parameter 
with  change  in  contact  friction  has  not  yet  been  fully  characterized.  These  areas 
suggest  opportunities  for  further  research. 


8-3 


One  final  note  concerning  the  physical  implementation  of  a  grasping  robot.  As 
mentioned  in  Chapter  II,  hardware  requirements  include  high  fidelity  contact  force 
sensors  and  force  accuators.  Without  these  elements,  adequate  control  of  the  grasped 
object  may  not  be  possible.  Limitations  on  contact  force  output  from  the  grasping 
manipulators  may  be  modeled  in  force  space  in  addition  to  the  friction  constraint 
surfaces. 
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Appendix  A.  FLRS  Extensions 


This  appendix  documents  some  of  the  extensions  to  the  FLRS  algorithm.  The 
first  enhancement  is  the  ability  to  smoothly  engage  and  disengage  contacts  from  a 
grasped  object.  This  ability  is  crucial  in  order  to  accomplish  grasp  and  regrasp  of  an 
object.  The  second  extension  is  the  adjustment  of  the  contact  friction  cones  in  order 
to  have  some  measure  of  robustness  with  regards  to  contact  stability  in  the  face 
of  random  or  unexpected  perturbations.  Another  extension  involves  mixed-mode 
contacts,  where  conventional  point  contacts  with  friction  are  used  in  conjunction 
with  bi-directional  contacts.  Bi-directional  contacts  depend  on  structural  load  paths 
rather  than  friction  constraint  to  maintain  contact  with  the  grasped  object.  The 
last  extension  discusses  alternative  definitions  of  the  FLRS  fuzzy  rulebase  used  for 
weighting  the  internal  forces. 

A.l  Transitional  Contacts 

The  FLRS  algorithm  may  be  modified  to  allow  contact  transitions  that  create 
force  closure  grasps  with  varying  number  of  contacts.  That  is,  contacts  may  be 
engaged  and  disengaged  from  the  grasped  object  in  a  controlled  manner.  Recall, 

Q  =  WF  (A.l) 

where  F  =  [  f ^  £2  •  •  •  ^  is  the  grasp  matrix.  For  a  given  contact  i,  let 

sJ[t)  denote  the  transition  weight  variable,  sji{t)  €  [  0  1  ]•  sJ,{i)  corresponds  to 
the  level  of  contact  engagement.  A  contact  that  is  fully  engaged  is  defined  to  have 
sJ.[t)  =  1,  while  a  contact  that  is  fully  disengaged  is  defined  to  have  sji{t)  =  0.  Let 

sJ{t)  0  0 

SJ(f)  =  0  sji{t)  0  (A.2) 

0  0  sJ[t) 
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We  may  now  form  the  weighted  grasp  matrix  as 


^  E3  •  •  •  S  j(t)E3  ■  •  ■  E3  \ 
•••  SA{t)Pi  Pm) 


(A.3) 


Thus,  for  a  given  object  wrench  and  contact  transition  state,  we  may  calculate 
the  weighted  external  force  vector  as:  Fext  =  WsJ(t)’^Q.  The  external  forces  so 
determined  reflect  a  minimum  norm  solution  of  the  weighted  contact  forces.  This 
provides  the  basis  for  the  FLRS  algorithm  to  properly  calculate  the  total  transitional 
contact  forces. 

During  the  er_fi  calculation,  the  convergence  cone  offset,  6x,  for  the  contact 
is  redefined  as:  SxsJ{t)  =  sJ(t)*Sx.  Thus  the  convergence  cone  and  er_fi  transition 
with  the  contact.  The  last  change  to  the  FLRS  algorithm  is  the  redefinition  of 
the  change  in  internal  force, 


m 

Afsi(t)  =  sJ{t)  *  Wij{t)  ■  eJij  (A. 4) 

j=i 

this  limits  the  use  of  internal  forces  associated  with  the  contact. 

A  contact  transition  is  composed  of  n  transition  steps.  Where  n  is  the  number 
of  discrete  solutions  calculated  during  the  transition.  This  value  should  depend  on 
urgency  of  the  transition,  the  robustness  of  the  contact  solutions  (discussed  later), 
and  the  force  control  capabilities  of  the  manipulators  in  general.  Each  solution  gen¬ 
erated  will  be  consistent  with  the  overall  FLRS  algorithm,  i.e.  a  minimum  norm  type 
solution  for  the  rulebase  described  in  this  paper.  During  each  step  of  the  contact 
transition,  the  pseudoinverse  of  the  weighted  grasp  matrix,  Ws_i(t),  will  be  calcu¬ 
lated.  This  additional  calculation  will  thus  slow  the  FLRS  algorithm.  However,  if 
the  number  of  transition  steps  is  defined  in  advance,  and  the  contact  to  be  transi¬ 
tioned  is  known,  all  the  pseudoinverse  calculations  may  be  accomplished  in  advance 
of  the  transition. 
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Contact  1 


Figure  A.l  Contact  one,  applied  force  during  contact  transition 


A.  1.1  Numeric  Example.  To  demonstrate  the  transition  capabilities  of  the 
FLRS,  the  five  contact  configuration  of  Figure  7.1  will  be  used.  The  example  will 
begin  with  Contacts  1  and  3  providing  the  contact  forces  necessary  to  stably  grasp 
the  object  and  generate  the  object  wrench,  Q  =  [  —.707  —.707  0  0  0  0  ]^- 
Contact  2  will  be  transitioned  first  to  form  a  three  contact  grasp;  likewise,  Contacts 
4  and  5  will  be  transitioned  into  contact  with  the  object,  in  turn.  At  any  given 
moment  during  the  contact  transitions,  the  object  wrench  remains  the  same.  The 
solutions  generated  by  the  FLRS  algorithm,  during  the  transitions,  will  exhibit  the 
standard  minimum  norm  behavior  as  before.  Ten  steps  will  be  used  for  each  contact 
transition.  Figures  A.l- A. 5  illustrate  the  change  in  contact  force  for  all  the  contacts 
during  the  transitions. 
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Contacts  Fully  Engaged 


Figure  A. 3  Contact  three,  applied  force  during  contact  transition 
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Contacts  Fully  Engaged 
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Figure  A. 4  Contact  four,  applied  force  during  contact  transition 


Contact  5 


Contacts  Fully  Engaged 


Figure  A. 5  Contact  five,  applied  force  during  contact  transition 


2.2 


Norm  of  the  Contact  Force 


1.8 


o 

O 

o  1.4 

E 

o 

Z 

1,2 


0.8 


+  + 


+++++++ 


1&3 


1-3  1-4 

Contacts  Fully  Engaged 


1-5 


Figure  A. 6  Norm(F),  during  contact  transition 


A. 2  Robust  Contact  Stability 

During  implementation  of  any  control  algorithm,  system  noise,  from  various 
sources,  is  present.  Generally,  designers  must  assume  some  characterization  for  the 
noise  and  design  a  control  system  which  is  stable  in  the  face  of  such  noise.  The 
grasp  force  assignment  algorithms  will  face  similar  challenges.  Nakamura  has  devel¬ 
oped  equations  which  relate  friction  cone  constraints  with  specified  levels  of  contact 
stability  [40].  A  disturbance  to  the  system  is  assumed  to  be  an  instantaneous  accel¬ 
eration  of  the  grasped  object.  An  equivalent  contact  disturbance,  f^,  may  instead  be 
assumed  to  exist  and  have  a  known  maximum  magnitude  such  that: 


llfrfll  <  flc  (A.5) 

Nakamura  then  forms  a  contact-stability  cone,  similar  to  the  FLRS  convergence  cone. 
The  analogous  offset,  the  distance  the  contact-stability  cone  is  moved  inward  along 
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Figure  A. 7  Contact-stability  cone 

the  local  Xj  axis,  is  _ 

llfdll  =  (A.6) 

Figure  A. 7  illustrates  the  contact-stability  cone.  Thus  disturbances,  up  to  magnitude 
Oc,  may  be  superposed  on  the  contact  force  solution  and  the  net  contact  force  will 
still  have  contact  stability.  This  development  assumed  a  known  constant  maximum 
value  of  disturbance,  independent  of  applied  contact  forces.  If  one  wanted  to  also 
assume  a  linear  relationship  between  actuator  noise  and  actuation  force,  one  could 
develop  equations  similar  to  Equations  A. 5  and  A.6  but  with  an  additional  con¬ 
straint  on  the  cone  angle  of  the  contact-stability  cone,  as  illustrated  in  Figure  A. 8. 
This  enhanced  contact-stability  cone  would  then  allow  increasing  levels  of  actuator 
noise  with  commanded  actuator  output,  where  a^.  =  am(fO-  The  FLRS  algorithm 
would  substitute  the  contact-stability  cone  for  the  friction  cone,  in  the  previous  de¬ 
velopment,  and  establish  a  convergence  cone  similar  to  the  contact-stability  cone  but 
displaced  a  further  6x  along  the  local  x,  axis. 
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Figure  A. 8  Contact-stability  cone  with  actuator  disturbance 
A.S  Mixed-mode  Contact  Grasps 


Mixed-mode  contact  grasps  are  comprised  of  both  conventional  point  contacts 
with  friction  constraints,  and  bi-directional  contacts.  Bi-directional  contacts  depend 
on  structural  load  paths  rather  than  friction  constraint  to  maintain  contact  with 
the  grasped  object.  Thus  they  are  contacts  which  do  not  need  any  internal  forces, 
and  in  fact  should  not  have  any  for  a  minimum  norm  solution.  The  grasp  force 
assignment  solution  proceeds  in  the  same  way  the  standard  solution  of  Chapter  IV, 
except  that  for  any  bi-directional  contact,  i,  er_f,(f)  =  0  and  erMotij  =  0  V  j.  Thus, 
any  non-zero  internal  force  pair  associated  with  a  bi-directional  contact  is  the  result 
of  internal  force  requirements  of  a  conventional  point  contact  associated  with  the 
internal  force  pair. 
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A. 4  Alternate  FLRS  Rulebase 


The  FLRS  algorithm  uses  a  fuzzy  inference  method  for  the  weighting  function, 
Wij.  Chapter  V  showed  that  convergence  of  the  FLRS  algorithm  is  dependent  on  the 
use  of  an  appropriate  weighting  function;  one  could  use  an  analytic  function  of  the 
type 


Wij{i) 


(er-dotij  +  erjdotjiY 
2" 


(A.7) 


where  n  is  a  positive  odd  integer.  However,  current  research  has  proven  that  through 
the  use  of  a  fuzzy  proportional  controller,  proportional-plus-derivative  response,  for 
a  second  order  system  can  be  obtained  [45].  Thus,  one  can  seek  higher  performance 
solutions  with  proportional  data  than  a  conventional  proportional  control  law  would 
allow.  The  fuzzy  logic  rulebase,  and  associated  antecedent  and  consequent  sets,  al¬ 
low  for  many  degrees  of  freedom.  Thus,  one  may  seek  varying  solution  behaviors  by 
changing  the  many  parameters  which  govern  the  solution.  Typically,  one  would  spec¬ 
ify  a  desired  behavior  in  terms  of  an  objective  function  and  use  a  genetic  algorithm 
to  determine  a  suitable  fuzzy  controller. 
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Appendix  B.  MATLAB  Script 


This  appendix  documents  the  MATLAB  script  that  was  used  to  generate  the 
data  presented  in  this  document.  This  includes  the  code  necessary  to  generate  object 
wrenches  and  obtain  solutions  through  the  FLRS  algorithm.  The  code  is  rough.  The 
main  code  is:  main.fnl  This  code  calls  other  script  files  and  MATLAB  functions, 
some  were  developed  for  this  research  while  others  are  standard  MATLAB  functions. 
Any  standard  MATLAB  functions  will  not  be  included  in  this  appendix.  Inside 
mairi-fnl  are  a  number  of  flags  which  control  solution  and  display  modes.  Hopefully, 
anyone  with  the  fortitude  of  an  ox  will  be  able  to  determine  what  the  options  are 
and  how  to  access  them. 

B.  1  mainjnl 

'/.this  is  the  example  contact  force  assignment  method 
'/.This  code  allows  individual  weighting  of  contacts, 

'/.which  allows  contact  transitions,  and  can  be  used  to 

'/.determine  minimum  friction  cone  offsets  for  a  given  set 

'/.of  numerical  optimization  termination  criteria.  This 

'/.code  will  also  allow  mixed  mode  contacts  (uni-directional  and 

'/.bi-directional) .  The  capability  to  capture  the  d_F  data  as  produced 

'/.will  allow  analysis  and  graphical  presentation  of  the  solution 

'/.process.  Also  capable  of  calling  the  Nakamura  solution 

'/.  Mark  Hunter,  25  Feb  96,  Air  Force  Institute  of  Technology,  WPAFB,  OH. 

clear 

global  T_i  n  in  mu  mu2  eta  del_X  fc_x  dX  e_I  e_Ic  num_I  RB_ef  c_dot 
cfd.wt  afd_wt  e_n_oi  o.flag  CT  BC  F_IT  E_IT  numit  Ap.flag  Fp_flag  r  Q 
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'/,  if  Ap_flag  is  on  (1),  plot  graphical  contact  force  convergence  process 
Ap_flag=0; 

*/,  if  Fp.flag  is  on  (1),  plot  F(i)  vs  i  contact  force  convergence  process 
Fp_flag=0  ; 

*/,  coeffecient  of  friction 

mu= . 4 ; 

mu2=mu“2; 

mu3=(l+mu2) “3/ (2*mu2) ; 
eta=l/sqrt(l+mu“2) ; 

*/,  call  contact  initialization  routine 
cf_ini;  '/contact  information 

del_X=ones(n,l)*.05;  '/friction  cone  displaced  in  local  x-dir  for 
'/convergence  reasons 

fc_x=ones(n,  1)*0;  '/additional  friction  cone  displacement  for 
'/robust  stability 

'/  maximum  and  minimum  normal  force,  not  used  yet 
'/Fn_max=4; 

'/Fn_min= .  1  ; 

'/  call  grasp  initialization  routine 
gf.ini 

'/  initialize  variables 

in=l :n; 
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num_I=(n*n-n)/2;  '/.number  of  internal  forces 


'/,  define  the  rule-bases  with  which  the  fuzzified  contact  information 
'/,  will  be  compared  with 

RB_ef=  [  1  1  1  2  2  3  4;  '/.l-Neg  Lrg 
1  2  2  3  3  4  5;  •/.2-Neg  Med 

1  2  4  4  4  5  6;  '/.S-Neg  Sml 

2  3  4  4  4  5  6;  •/.4-Zero 

2  3  4  4  4  6  7;  •/.5-Pos  Sml 

3  4  5  5  6  6  7;  '/.e-Pos  Med 

4  5  6  6  7  7  7];  '/.7-Pos  Lrg 

I6=eye(6) ; 

A=null(W);  '/.MATLAB  'null’  command,  calculates  orthonormal  basis 
'/.for  null  of  W,  used  in  CONSTR  solver 
b=(3*n)-rank(W) ; 

x_old=ones(b,l)/b;  '/.used  in  CONSTR  solver  as  initial  guess 

'/.define  bi-directional  contacts,  BC(i)  =  (l  or  0) 

'/.thus  the  contact  can  support  forces  in  all  directions  but  no  moments 

BC=ones(n,  1) ;  '/.default  setting  is  to  have  all  contacts  uni-directional 
nct=10;  '/.number  of  steps  to  transition  a  contact 
'/.  begin  input  data  cycle 

'/.for  it=l:l  '/.number  of  solutions  sought  for  this  run 
for  it=l:72 
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it 

'/.pause 

'/.for  ait=l:6  '/.use  with  force  closure  test 
'/.for  sit=l:2  '/.use  with  force  closure  test 
'/.it=(ait-l)*2+sit;  '/.use  with  force  closure  test 

'/.define  input  object  wrench 
th=(it)*pi/36; 
f_net=Ccos(th)  sin(th)  0]; 

'/.  f_net=[l  1  0]; 
m_net=[0  0  0]; 

Q=[f_net  m_net] ’ ; 

'/.random  wrench  test 
'/.  Q=(rand(6,l)-.5)*2; 


'/.calculate  the  12  manipulation  unit  vectors,  force  closure  test 
'/.if  sit=*l 
'/.  q=I6(:,ait); 

'/.else 

'/.  Q=-I6(:  ,ait) ; 

'/.end 

'/.define  contact  transition  vector,  n  elements  0<=CT(i)<=l 
CT=ones (n,  1) ;  '/.default  setting  is  to  have  all  contacts  fully  engaged 
'/.  CT(5)  =  (it-l)/nct;  '/.transition  contact  two 
'/.  CT(4)=0; 

'/.  CT(5)=0; 


Wt_W=W;  '/.copy  W  into  Wt_W  to  allow  for  weighted  grasp  matrix 


B-4 


dX=del_X+f c_x;  '/,del_X  is  the  convergence  buffer,  fc_x  is  the 
*/, stability  offset 

if  min(CT)<l  ’/.then  at  least  one  contact  is  transitional 
for  i=l:n 
if  (CTCiXl) 

Wt_W(: ,(i-l)*3+l:i*3)=Wt_W(: ,(i-l)*3+l:i*3)*CT(i); 

dX(i)=dX(i)*CT(i); 

end  */,if 

end  '/.for 

end  ’/.if 

’/.pi_flops=flops;  ’/.zero  counter 

Ff=pinv(Wt_W)*Q;  ’/.get  psuedoinverse  solution  (external  forces) 
Fo=Ff ; 

’/.pause 

'/.pi_flops=flops-pi_flops  ’/.report  number  of  flops  for  psuedoinverse 
flops(O);  ’/.zero  flops  counter 

flrs_flg=l; 

if  flrs_flg==l  ’/.calculate  FLRS  solution 

’/.  Fh=f_sol(Ff  ,Cf  ,wt_it) ;  ’/.call  fuzzy  solver  as  script 

f_sol;  ’/.call  fuzzy  solver  as  function 
Fh=F; 
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*/,if  algorithm  plot  switch,  Ap.flag,  is  on, 

'/.plot  the  contact  force  convergence; 
if  Ap_flag==l 
a2_plot(F_IT,numit ,E_IT) ; 
end  */,if 

'/.if  contact  force  history  plot  switch  is  on,  do  it 
if  Fp_flag==l 
fp_plot(F_IT,numit) ; 

[mi,ni]=size(F_IT) ; 
nn=l :ni; 
fs=[nn'  F.IT']; 

'/.  save  dx05q_2c.dat  fs  -ascii  -tabs 
end 

'/,  Fh  is  a  3  X  n  matrix  of  contact  forces 
h_fpos=flops;  '/.record  FPOs 

end  '/.flrs_flg 

'/.save  data  to  files  for  analysis 
if  flrs_flg==l 
hm_fpos (it)=h_fpos/l . e6 ; 
for  i=l:n 

F_h(i*3-2:i*3,l)=Fh(: ,i); 
end 

'/.  Qh_app(:  ,it)=W*F_h; 

Ch_save( : ,it)=F_h; 
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end 


if  l_flag==l 

nlm_f pos (it) =nl_f pos/1 . e6 ; 

Cnl_save( : ,it)=Fn_l; 
end 

end  y,end  input  loop 
if  flrs_flg==l 

save  diss_h5  Ch_save  hm.fpos  y,Q_com 
y,  save  dis_vmu4  Ch_save  hm.fpos 
end 


B.1.1  cfJni.m. 

y,  this  is  the  contact  initialization  routine  for  the  main.fl.m  script 

y,  degrees  to  radians 
deg2rad=pi/180; 

y,  establish  object  base  coordinates 
x=[l  0  0]’; 
y=[0  1  0]>: 
z= [0  0  1]  ’ ; 


y,  enter  number  of  contacts 

y.n=2 ; 

n=3; 

y.n=4; 

y.n=5; 
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y, assume  object  is  imit  sphere 
rho=l; 

'/,  looking  a  the  grasped  object  model, 

'/.the  x-z  plane  forms  the  zero  meridian 
'/,  azimuth  (theta)  (j)  measured  from  z-axis 
'/,  the  z-x  plane  forms  the  equator  with 
'/.positive  elevation  up  towards  y-axis, 

'/,  or  North  pole. 

'/,  units  in  degrees 

*/,azm=  [90  -90]  ; 

azm=[  90  0  -90];  */,r&a96  3  contacts 
'/,azm=[  90  0  -90  0];  */,r&a96  4  contacts 
*/,azm=[  90  0  -90  0  180];  y,r&a96  5  contacts 

'/.ele=  [0  0]  ; 

ele=[0  -90  0];  */,r8:a96  3  contacts 
'/,ele=[0  -90  0  45];  */,r&a96  4  contacts 
*/,ele=[0  -90  0  45  45];  '/,r&a96  5  contacts 

'/,azm=[  90  -90  0  0]  ; 

•/,ele=  [  0  0  90  -90]  ; 

'/,  convert  units  to  radians 

azm=azm*deg2rad; 
ele=ele*deg2rad ; 

'/,  calculate  contact  positions  in  x-y-z  coordinates  of  r 
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for  i=l:n 

r(l,i)=rho*sin(azm(i))*cos(ele(i)) ; 
r(2,i)=rho*sin(ele(i)) ; 
r(3,i)=rho*cos(azm(i))*cos(ele(i)) ; 

y,  contact  normal  (contact  x-axis)  in 
'/object  frame,  for  a  sphere  is  easy 
*/,  (negative  unit  vector  of  position  vector,  r) 
e_n_oi( : ,i)=-r( : ,i)/norm(r( : ,i)) ; 

*/,  determine  appropriate  teingent  vectors  to  contact  surface 

e_t( : , l)=cross(e_n_oi( : ,i) ,x) ; 
e_t( : ,2)=cross(e_n_oi( ; ,i) ,y) ; 
e_t( : ,3)=cross(e_n_oi( : ,i) ,z) ; 

for  j=l:3 

norm_e_t(j)=norm(e_t( : ,j)); 
end 

i_et=max (f ind(norm_e_t==max(norm_e_t) ) ) ; 
e_t_l( : ,i)=e_t(: ,i_et)/norm(e_t( : ,i_et)) ; 

e_t_2( : ,i)=cross(e_t_l( : ,i) ,e_n_oi( : ,i)) ; 
e_t_2( : ,i)=e_t_2( : ,i)/norm(e_t_2(: ,i)) ; 


T_i(3*i-2:3*i,l:3)=. . . 

[e_n_oi(:,i)  e_t_l(:,i)  e_t_2(:,i)]; 
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end 


B.1.2  gfJni.m. 

'/,  Mark  Hunter,  25  Feb  96,  Air  Force  Institute  of  Technology,  WPAFB,  OH. 
*/,  this  code  defines  the  fuzzy  sets  and  domains. 

'/,  also  defines  internal  force  unit  vectors  and  grasp  matrix 
clear  W 

*/, vectors  of  center  of  triangular  functions  for  input  domain 
c_dot=[-l  -.6  -.2  0  .2  .61]’; 

'/,c_dot  [Ability  of  Internal  force  to  affect  error_force] 

*/,plot_mem(c_dot ,  ’  Internal  .Error  ’ )  ; 

*/, vector  of  fuzzy  singletons  for  Sugeno  type  output  domain 
cfd_wt=[-l  -.75  -.35  0  .35  .75  1]’; 

'/.cf d.wt=  [-Lrg.Wt  -Med.Wt  -Sml.Wt  No-Wt  Sml.Wt  Med.Wt  Lrg.Wt] 

’/,  hardwire  weights  (areas)  of  cfd.wt 
afd_wt=[l  111111]’; 

d2r=pi/180;  */, degree  to  radian  conversion 

e_y=C0  10]’;  '/.establish  global  basis  vectors 
e_x=[l  0  0]’; 
e_z=[0  0  1]’; 

'/,  establish  internal  force  unit  vectors 
'/.and  their  weights  associated  with 
'/.  the  fuzzy  membership  functions 

for  i=l:n 
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for  j=l:n 

jj=(i-l)*n+j ; 
if  i==j 

e_I(: 00]’; 
else 

r_ij=r(: ,i)-r(: . j) : 

e_I(:  ,jj)=-r_ij/norm(r_ij) ;  ‘/.internal  force  unit  vectors 

end 

e_Ic(: ,jj)=T_i(3*i-2:3*i,l:3)’*e_I(:,jj); 

'/ie_I  in  appropriate  contact  frame 

end 

end 

*/,  begin  initialization  for  pseudo-inverse  solution/external  force  solution 
I3=eye(3,3) ; 

for  i=l:n 

evaKE’P’  int2str(i)  ’  =  [0  -r(3,’  int2str(i)  ’)  r(2,’  int2str(i)  ’); 
r(3,’  int2str(i)  ’)  0  -r(l,’  int2str(i)  ’); 

-r(2,’  int2str(i)  ’)  r(l,’  int2str(i)  ’)  0  ];’]) 
end 

for  i=l:n 

eval([  ’Wi=Cl3;P’ ,int2str(i) , ’  ]); 

W=CW  Wi]  : 
end 

Wp=pinv(W) ; 

'/.end  initialization  routine 
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B.1.3  f.sol.m. 


'/.This  is  the  heart  of  the  FLRS  algorithm 
'/.function  F=f _sol(Fo,Cf  ,wt_it) 

'/.global  T_i  n  in  mu  mu2  mu3  eta  del.X  fc_x  dX 
'/.e_I  e_Ic  num_I  RB_ef  c_dot  cfd.wt  afd_wt  e_n_oi 
'/.o_flag  CT  BC  F_IT  E_IT  numit  Ap.flag  Fp.flag 
iq=zeros(3,l) ; 

n_max=30;  '/.maximum  number  of  iterations  allowed 
'/.n_max=25;  '/.for  display  purposes 

numit=0;  '/.initialize  number  of  iterations  until  solution 

if  Ap_flag==l  I  Fp_flag==l 

'/.record  Fc  so  that  the  change  in  contact 

'/.  force  may  be  graphically  displayed 

F_IT=zeros(3*n,n_max) ; 

E_IT=zeros(3*n,n_max) ; 
end 

C=l;  '/.initialize  scale  factor  C 
C_old=l; 

for  i=l:n 

F_e(: ,i)=Fo((i-l)*3+l:(i*3)); 

end 

'/.  determine  the  error  between  current  commanded  contact  force 

'/.  and  actual  possible  contact  force.  Use  shortest  path  to  the  friction 

'/.  cone  as  the  basis  for  the  error. 

F_i_old=zeros(3,n) ; 
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'/,  calculate  current  contact  forces 
F(: ,in)=F_e(: , in)+F_i_old( : ,in) ; 

for  i=l:n 

Fc( :  ,i)=T_i(3*i-2:3*i,l  :3) ’*F(:  ,i) ;  */, convert  to  local 
*/, frame 

theta(i)=atan2(Fc(3,i)  ,Fc(2,i))  ;  '/.angle  closest  to 
'/.original  F_e 
end 

•/.Fc 

numit=numit+l ; 
if  Ap_flag==l  I  Fp_flag==l 

'/.record  Fc  so  that  the  change  in  contact  force  may  be  graphically 
'/.displayed 
for  ic=l:n 

F_IT(ic*3-2:ic*3,numit)=F(: ,ic) ; 

end 

end 

'/.  is  any  current  contact  force  outside  of  friction  cone 
for  i=l;n 

f c_check(i)=-e_n_oi( : ,i) ’*F(: ,i)+eta*norm(F(: ,i)) ; 
end 

'/.  begin  iterative  loop  to  find  solution 
while  (max(f c_check)>0  &  (numit-l)<n_max) '/.contact 
'/.stability  is  violated 

'/.  determine  what  the  goal  vector  should  be  based  on  the  direction 
'/.  of  F_e 
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for  i=l:n 

'/,  calculate  error  based  on  current  F  and  goal 
if  f c_check(i)<=0 
ER_fc(: ,i)=zeros(3,l) ; 
else 

y,  calculate  closest  point  on  friction  cone  from  present  contact  force, 
dx_p=(Fc(l,i)+mu*(  Fc(2,i)*cos(theta(i))  +... 

Fc(3,i)*sin(theta(i)) . . . 

)+mu2*dX(i)  )/(l+mu2); 

'/,  make  any  dx_*<dX  equal  to  dX 
dx=dx_p*(dx_p>=dX(i) )+dX(i)*(dx_p<dX(i)) ; 

ER_fc( : ,i)=[dx 

(dx-dX(i) )*mu*cos (theta (i)) 

(dx-dX(i))>i‘mu>t<sin(theta(i))]-Fc( :  ,i) ; 

end 

y, convert  to  global  frame 
ER_f (: ,i)=T_i(3*i-2:3*i,l:3)*ER_fc(: ,i) ; 
nER_f (i)=norm(ER_f ( : ,i) ) ; 
end 

if  Ap_flag==l  1  Fp_flag==l 

'/.record  ER_f  so  that  the  change  in  contact  force  may  be 
'/.graphically  displayed 
for  ic=l:n 

E_IT(ic*3-2:ic*3,numit)=ER_f (: ,ic) ; 
end 
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end 


'/,  loop  through  all  the  internal  forces 

•/. 

wt_tot=zeros(l ,n*n) ; 

ER_dot=zeros(n*n,l) ; 

for  i=l:n 
for  j=l:n 
jj=(i-l)*n+j ; 

y,  calculate  the  dot  product  between  the  contact  error  and  the 

y,  internal  force  for  each  contact 

y,  ER_dot(j  j)=dot(e_Ic( : ,  j  j)  ,ER_fc(:  ,i))  ; 

y,do  this  in  local  frame 

ER.dot (j j )=dot(e_I( : , j j ) ,ER_f ( : ,i)) ; 

y,try  this  in  global  frame,  shouldn’t  make  any  difference 

y,  if  the  finger  being  faded  in  or  out  is  i,  then  weight  the 

y,  associated  ER.dot’s 

ER_dot ( j  j ) =ER_dot ( j  j ) *CT ( j ) *BC ( j ) ; 

end 

end 

'/.determine  max  value  of  ER_dot 

ER_  dot  _m=max (max ( ab  s ( ER_  dot ) ) ) ; 
ER_dot_m=ER_dot_m*(ER_dot_m>0)+(ER_dot_m==0) ; 

'/normalize  ER_dot 
ER_dot=ER_dot/ER_dot_m; 
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tst_flag=0;  '/.test  the  use  of  analytic  function  rather  than  fuzzy 


if  tst_flag==0 

'/,  calculate  memberships  of  the  ER_dot  scalars 
[mf _ERd ,mu_ERd] =mem_edci (ER.dot , c_dot ) ; 

'/,  now  apply  the  rule  base  to  the  input  data  to  weight  the 
'/,  individual  internal  forces. 
wt_ef =zeros (7 ,num_I) ; 

'/,  wt=zeros(7,num_I) ; 

l_count=0; 

for  i=l:n-l 

for  j=l:n 

if  i'=j  &  i<j 

jj=(i-l)*n+j : 

ii=(j-l)*n+i: 

I_count=I_count+l ; 

for  1=1:2 
for  k=l:2 

i_wt_mf=RB_ef (mf_ERd(j j ,1) ,mf_ERd(ii,k)) ; 
wt_ef (i_wt_mf ,I_count)=wt_ef (i_wt_mf ,I_count)+. • . 
min(mu_ERd(j j ,1) ,mu_ERd(ii,k) ) ; 

end 

end 

end 

end 

end 
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7.wt_ef 

wt_tot=(((afd_wt.*cfd_wt) ’*wt_ef) . /(afd.wt ’ *wt_ef ) ) ; 

*/,wt_tot 

'/.define  S3rmetric  internal  force  weighting  matrix 

l_count=0 ; 

for  i=l:n-l 

for  j=l:n 

if  i"=j  &  i<j 

I_count=I_count+l ; 

w(i, j)=wt_tot(I_count) ; 

w(j ,i)=wt_tot(I_count) ; 

end 

end 

end 

•/.w 

else  */,tst_flag“'=0 

'/.this  is  a  trial  method  to  determine  what  interaction  the  internal 

'/.forces  should  have  without  using  fuzzy  logic 

kk=0; 

l_count=0; 
for  i=l:n-l 
for  j=l:n 
if  i"=j  &  i<j 
I_count=I_co\int+l ; 
jj=(i-l)*n+j ; 
ii=(j-l)*n+i; 

if (nER.f ( i) +nER_f ( j ) ) ==0 
wt_tot(l_count)=0; 
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else 


d_er=ER_dot(j j)+ER_dot(ii) ; 

wt_tot(I_count)=d_er/(2+l/abs(d_er)“kk)*. . . 

(2+.5"kk)/2; 

end 

end 

end 

end 

'/,  [ER.dot  wt_tot’] 

end  y,end  tst_flag 

d_F=zeros(3,n) ; 
l_count=0; 
for  i=l:n-l 
for  j=l:n 
if  i''=j  &  i<j 
I_count=I_count+l ; 
jj=(i-l)*n+j ; 
ii=(j-l)*n+i; 

d_F(: ,i)=d_F(; , i)+wt_tot (I_count)*e_I( : ,jj); 
d_F(: ,j)=d_F(: , j )+wt_tot(I_count)*e_I( : ,ii); 
end 
end 

end 

'/,  keep  track  of  last  three  contacts  which  have 
*/,  had  greatest  dot(d_F,ER_f ) 
iq(3)=iq(2) ; 
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iq(2)=iq(l);  '/.update  last  iq 

'/.find  contact  corresponding  to  the  largest  dot  (d_F  ,ER_f ) 
chk=0 ; 
for  i=l:n 

dF_ER(i)=dot (d_F( : , i) ,ER_f ( : , i) ) ; 

if  dF_ER(i)>chk 

chk=dF_ER(i) ; 

iq(l)=i; 

end 

end 

'/.keep  track  of  past  iq,ER_f  ( :  ,iq)  ,  and  d_F(:,iq) 

'/.if  iq  doesn’t  change, 

if  n;imit>3  ft  iq==Ciq(l)  iq(l)  iq(l)]’ 
c_flag=6; 

'/.  c_flag=2; 
else 

c_f lag=2; 
end 

'/.  determine  C,  in  order  to  acheive  least  squares  solution  (min  norm) 
if  c_flag==l 

C=pinv(d_F(:  ,iq(l)))>KER_f  (:  ,iq(l))  ; 

'/.  since  inputs  to  pseudoinverse  is  a  vector, 

'/.we  can  simplify  this  procedure 
elseif  c_flag==2 

C=(d_F(: ,iq(l))’*ER_f(:,iq(l)))/(d_F(:,iq(l))’*d_F(:,iq(l))); 

'/.  if  d_F  is  not  along  ER_f ,  then  C  will  be  small,  not  much  will  happen 
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'/,  this  is  the  case  for  slow  converging  problems,  let’s  change  the 

'/,  error  used  to  calculate  C,  by  adding  d_F  to  ER_f  by  an  amount  inversely 

y,  proportionally  to  the  dot  product  of  the  two. 

elseif  c_flag==6 

(’used  new  C  algorithm’) 

C=(norm(ER_f ( : ,iq(l)))“2)/. . . 

(dot(d_F(:.iq(l)),ER_f(:.iq(l))  )  ); 
end  '/.end  c_flag  if 


•/ V  •/•/•/ V  VV  VV  VV  VV  •/•/•/•/•/•/•/•/</•/•/•/ VV  V  •/•/•/•/•/ V  V  •/•/ •] 


•  h  It  It  It  It  It  It  It  It  It  It  It  It  It  It  It  It  It  It  It 


y, calculate  maximum  value  of  C  allowed 
'/(and  meet  convergence  reqt’s) 

'/the  calculation  below  assumes  that  f(i)  lies 
'/in  zone  B1  of  th  contact  force  zones 


num=0; 
den=0 ; 
l_count*0; 
for  i=l:n 
for  j=l:n 
if  i'=j  &  i<j 


I_count=I_count+l ; 


jj=(i-l)*n+j ; 
ii=(j-l)*n+i; 

num=num+w(i, j)*ER_dot(j j)+w(j ,i)*ER_dot(ii) ; 
'/this  is  like:  w_ij*er_dot_ij+w_ji*er_dot_ji 
end 
end 

den=den+sum(w(i, :))“2; 
end 
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C_max=num/ den*mu3 ; 


'/.  [C  C_max] 

if  C>C_max 

(’C  exceeded  C_max  and  was  corrected’) 

y.  C=C_max; 

end 

y,  update  F_i 
F_i_old=F_i_old+C*d_F ; 

y,  calculate  current  contact  forces 
F( : ,in)=F_e( ; ,in)+F_i_old(: ,in) ; 

for  i=l:n 

Fc(: ,i)=T_i(3*i-2:3*i,l:3)’*F(:,i); 

'/.convert  to  local  frame 
theta(i)=atan2(Fc(3,i) ,Fc(2,i)) ; 

'/.update  angle 
end 

numit=numit+l ;  '/.increment  interation  number 
if  Ap_flag==l  I  Fp_flag==l 

'/.record  Fc  so  that  the  change  in  contact  force  may 
'/.be  graphically  displayed 
for  ic=l:n 

F_IT(ic*3-2:ic*3,numit)=F(: ,ic) ; 

end 

end 
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'/,  is  any  current  contact  force  outside  of  friction  cone 

•/.F 

for  i=l:n 

f c_check(i)=-e_n_oi( : ,i) ’*F( : ,i)+eta*norni(F( : ,i)) ; 
end 

end  */,end  while  loop  for  contact  stability 

if  nuinit>=n_max 
numit=n_max; 

(’Solution  does  not  satisfy  constraints’) 
end 

B.1.4  mem-edci.m. 

y,  subroutine  which  calculates  the  membership  of  a  given  input  vector 
y,  with  respect  to  the  triangular  fuzzy  membership  functions  defined 
y,  by  the  vector  of  ’centers.’  Use  this  version  of  membership  calc- 
y,  ulation  for  external  force,  dot  product  of  internal  and  external 
y,  forces, 

y,  cross  product  of  internal  force  and  friction  cone  vectors,  and  output 
y,  weights  associated  with  each  internal  force, 
y,  See  cone.ctr.m 

y,  Mark  Hunter,  1  August  95,  Air  Force  Institute  of  Technology,  WPAFB,  OH. 

function  [mf , mu] =mem_edci(x, centers) 

y,mf  is  the  membership  function  label  of  the  first  non-zero  membership 
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*/,mu(i,l)  is  the  value  of  the  membership  of  x(i)  with  repect  to  mf(i) 
'/,index(i)  refers  to  the  number  of  membership  functions  crossed  by  x(i) 

n=max(size(x) ) ;  '/.number  of  input  data  scalars 
m=max(size(centers)) ;  */, number  of  membership  functions 

'/,  initialize  the  two  matrices 
mf=zeros(n,2) ; 
mu=mf ; 

for  i=l:n; 

mf _inc=max(f ind(x(i)>=centers)) ; 

if  mf_inc==C] 
mf (i, :)=[!  2] ; 
mu(i, :)=[!  0] ; 

elseif  mf_inc==m 
mf (i, : )  =  [m-l  m]  ; 
mu(i, :)=[0  1] ; 

else 

mf (i, :)=Cmf_inc  mf_inc+l] ; 

mu(i,l)=  (centers (mf_inc+l)  -  x(i))/... 

(centers(mf _inc+l)  -  centers (mf_inc) ) ; 
mu(i,2)=  (x(i)  -  centers (mf _inc))/ .. . 

(centers (mf_inc+l)  -  centers (mf_inc) ) ; 
end 
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end 


B.1.5  a2.plot.m. 


function  a2_plot(F_IT,numit ,E_IT) 

'/,  this  function  opens  figure  windows  for  each  contact  and  draws 
*/.  the  contact  forces 

'/,  as  the  FLRS  algorithm  calculates  the  next  iteration 
global  n  mu  T_i  r  Q 

y,  degrees  to  radians 
deg2rad=pi/180; 

colors=hsv(80) ;  '/set  colors  equal  to  matrix  of  default  color  map,  hsv 
bcolor®!;  '/base  contact  color 

'/figure  window  positions  and  sizes 
Pos=[  .33  .66  .33  .33; 

.66  0  .33  .33; 

.33  0  .33  .33; 

0  0  .33  .33; 

0  .4  .33  .33; 

.66  .4  .33  .33]; 

'/for  display  test  purposes 
'/numit=l ; 


'/display  object  as  a  unit  radius  sphere 
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f_hdl(l)=figure(l) ; 
clf 

set(f_hdl(l) , ’Units’ , ’normal’ , ’Position’ ,Pos(l, :) , ’MenuBar’ , ’none’  ) 
setCgca, ’NextPlot’ , ’add’) ; 

•/.plot  contact  surface  (lies  along  y-axis)  and  friction  cone 
•/.plot  in  x-y  plane 
ro=l ; 

for  po=l : 37 

theta=(po-l)*10; 

theta=deg2rad*theta ; 

sf cod, po)=ro*cos (theta) : 

sf co(2,po)=ro*sin(theta) ; 

sfco(3,po)=0; 

end 

so_hdl=plot3(sfco(l,:)’,  sfco(2,:)’,  sfco(3, :) ’ , ’c’) ; 

•/.draw  surface  line,  cyan 

set(so_hdl, ’LineWidth’ ,1) ;  •/.set  line  width  to  1 

axis([-l  1-11-11]) 
axis (’square’) ; 
axis (’off’) ; 
view(2) 

•/,  hold  on 

•/.plot  in  x-z  plane 

sfco(3, :)=sfco(2, :) ; 

sfco(2, :)=zeros(size(sfco(2, :))) ; 

so_hdl=plot3(sfco(l, ;) ’ ,  sfco(2,:)’,  sfco(3, :) ’ , ’c’) ; 
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'/.draw  surface  line,  cyan 

set(so_hdl, ’LineWidth’ ,1) ;  '/.set  line  width  to  1 
'/.plot  the  input  wrench,  Q 

q_hdl=arrow(Q(l:3) ,  [0  0  0]’);  '/.draw  wrench  yellow 
arrow (q.hdl,  ’Length MO,  ’LineWidth*  ,2, ’Color’ ,  ’y’) ; 
for  i=l:n 

'/.plot  solution  contact  forces 
cfs_hdl=arrow([0  0  0]+r( : ,i) ’ , . . , 
CF_IT((i-l)*3+l,numit)  F_IT((i-l)*3+2,nuinit) . . , 
F_IT((i-l)*3+3,numit)]+r(: ,i) ’) ; 

arrow (cfs.hdl, ’Length’ , 10, ’BaseAngle’ ,45, ’LineWidth’ ,2, 
colors (80,:)  ); 

'/.plot  both  halves  of  friction  cone  in  green 
k_fc=.5:  '/.scale  friction  cone 
'/.plot  in  x-y  plane 

fc_n=T_i(3*i-2:3*i,l;3)*Cl  -mu  0]’*k_fc; 
fc_p=T_i(3*i-2:3*i,l;3)*[l  mu  0]’*k_fc; 

fc_hdl=plot3([0  fc.n(l)]’+Cr(l,i)  r(l.i)]’,[0  fc_n(2)] ’ . 
+Cr(2,i)  r(2,i)]’,[0  f c_n(3)] ’+ [r(3,i)  r(3,i)] ’ , ’g’ ) ; 
fc_hdl=plot3([0  fc.p(l)]’+Cr(l,i)  r(l,i)]’,[0  fc_p(2)] ’ . 
+[r(2,i)  r(2,i)]’,[0  fc_p(3)] ’+[r(3,i)  r(3,i)] ’ , ’g’ ) ; 

'/.plot  in  x-z  plane 

fc_n=T_i(3*i-2:3*i,l:3)*[l  0  -mu]’*k_fc: 
fc_p=T_i(3*i-2:3*i,l:3)*[l  0  mu]’*k_fc; 

fc_hdl=plot3([0  fc_n(l)] ’+[r(l.i)  r(l,i)]’,[0  fc_n(2)] ’ . 


’Color’ , . 
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+[r(2,i)  r(2,i)3’,[0  fc_n(3)] ’+[r(3,i)  r(3,i)] ’ , ’b’ ) : 
f c_hdl=plot3( [0  fc_p(l)] ’+[r(l,i)  r(l,i)]’,[0  fc_p(2)] ’ . . . 

+Cr(2,i)  r(2,i)]’,[0  fc_p(3)] ’+[r(3,i)  r(3,i)] ’ , ’b’ ) ; 
end 

drawnow 

'/.this  function  plots  the  contact  force  convergence  history 
*/,for  each  CFA  solution 

'/.niuiiit  -  number  of  iterations  for  convergence 

'/,F_IT  -  matrix  of  contact  forces  applied  during  each  step  of  soln. 

*/,in  object  frame,  column  length  =  3*n,  row  length  =  numit 

*/, since  forces  are  in  local  contact  frame,  only  need  x  and  y  components 

'/.for  display  purposes . 

if  size(E_IT)>l 

e_flag=l;  '/.draw  in  the  contact  errors 
end 

for  i=l:n 

'/.  determine  axis  scale  to  show  the  force  changes  throughout 
'/.  the  convergence 

'/.  and  make  the  axes  square,  ie  all  axes  are  the  same  length 

xme=(F_IT((i-l)*3+l , 1 : numit )+E_IT((i- 1) *3+1, 1: numit ) ) ; 
yme=(F_IT((i-l)*3+2, 1 : numit )+E_IT((i- 1) *3+2,1 : numit)) ; 
zme= (F_IT( (i-1) *3+3 , 1 :numit)+E_IT( (i-l)*3+3 , 1 :n\miit) ) ; 
xm=F_IT((i-l)*3+l,l:numit) ; 
ym=F_ IT ((i-1) *3+2,1: numit) ; 
zm=F_IT((i-l)*3+3,l:numit) ; 

xmax=max ( [max (xm)  max (xme)  .  05] )  ; 
xmin=min ([min(xm)  min( xme )  -.05]); 
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ymax=max( [max(ym)  max(yme)  .05]); 
jnnin=min( Cmin(ym)  min(yme)  -.05]); 

2max=max  ( [max  (zm)  max  (zme)  .  05]  ) ; 
zmin=min( [min(zm)  min(zme)  -.05]); 

dx=xmax-xmin; 
dy=ymax-3rmin ; 
dz=zmax-zmin; 

if  dx>dy  &  dx>dz 
3rmax=ymax+  (dx-dy )  /2 ; 

3rmin=ymin-  (dx-dy)  /2 ; 
zmax=zmax+ (dx-dz) /2 ; 
zmin=zmin- (dx-dz) /2 ; 
elseif  dy>dx  &  dy>dz 
xmax=xmax+ (dy-dx) /2 ; 
xmin=xmin- (dy-dx) /2 ; 
zmax=zmax+ (dy-dz) /2 ; 
zmin=zmin- (dy-dz) /2 ; 
else  V,dz>dx  ft  dz>dy 
xmax=xmax+ (dz-dx) /2 ; 
xmin=xmiii-  (dz-dx)  /2 ; 
ymax=3rmax+  (dz-dy)  /2 ; 
ymin=ymin- (dz-dy) /2 ; 
end 

a_limit(i,  :)  =  [xmin  xmax  ymin  ymax  zmin  zmax]  ; 

f _hdl(i+l)=f igure(i+l) ; 

clf 

set(f_hdl(i+l) , ’Units’ , ’normal’ , ’Position’ ,Pos(i+l, :) , ’MenuBar’ , ’none’  ) 
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ab_hdl(i)=axes ; 

set(ab_hdl(i) , ’NextPlot’ , ’add’ ) 

axis(a_limit(i, :)) ; 

view(2) ; 

xlabelC ’Xo’ ) ; 

ylabel(’Yo’); 

zlabel( ’Zo’ ) ; 

axis (’square’) ; 

hold  on; 

'/plot  contact  surface  (lies  along  y-axis)  and  friction  cone 

'/plot  in  x-y  plane 

ri=nona(r( : ,i)) ; 

for  pi=l : 19 

theta= (pi- 1) *5+135 ; 

theta=deg2rad*theta ; 

sfc(l,pi)=ri+ri*cos(theta) ; 

sfc(2,pi)=ri*sin(theta) ; 

sfc(3,pi)=0; 

end 

csfc=T_i(3*i-2:3*i,l:3)*sfc; 

s_hdl=plot3(csf c(l , : ) ’ ,  csfc(2,:)’,  csf c(3, : ) ’ , ’ c ’ ) ; 

'/draw  surface  line,  cyan 

set(s_hdl, ’LineWidth’ ,2) ;  '/set  line  width  to  2 

'/plot  in  x-z  plane 

sf c(3, : )=sf c(2, : ) ; 

sfc(2, : )=zeros(size(sf c(2, :))) ; 

csf c=T_i(3*i-2:3*i, 1 :3)*sfc; 

s_hdl=plot3(csfc(l, :)  ’ ,  csfc(2,:)’,  csfc(3, :) ’ , ’c’) ; 

'/draw  surface  line,  cyein 
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set(s_hdl, ’LineWidth’ ,2) ;  '/.set  line  width  to  2 
'/.plot  large  white  arrows  to  indicate  point  contacts 

'/. 

pc_start=[-.2  00]’; 
pc_start=T_i(3*i-2:3*i, 1 :3)*pc_start ; 
pc_end=[0  00]’; 

pc_hdl=arrow(pc_start’ ,pc_end’) ; 

arrow (pc_hdl, ’Length’ ,30, ’LineWidth’ ,4, ’Color’ , ’w’) ; 


'/.plot  both  halves  of  friction  cone  in  green 

'/.plot  in  x-y  plane 
fc_n=T_i(3*i-2:3*i,l:3)*[l  -mu  0]’; 
fc_p=T_i(3*i-2:3*i,l:3)*[l  mu  0]’; 


fc_hdl=plot3( [0  fc_n(l)]’,C0  fc_n(2)]’,[0  f c_n(3)] ’ , ’g’ ) 
f c_hdl=plot3( [0  fc_p(l)]’,[0  fc_p(2)]’,[0  fc_p(3)] ’ , ’g’ ) 

'/.plot  in  x-z  plane 
fc_n=T_i(3*i-2:3*i,l:3)*[l  0  -mu]’; 
fc_p=T_i(3*i-2:3*i,l:3)*[l  0  mu]’; 

fc_hdl=plot3([0  fc_n(l)]’,C0  fc_n(2)]’,[0  f c_n(3)] ’ , ’b’ ) 
f c_hdl=plot3(  [0  fc_p(l)]’,[0  fc_p(2)]’,[0  fc_p(3)] ’ , ’b’ ) 

drawnow 


'/.plot  external  contact  force 
'/.  afo_hdl(i)=axes ; 

'/.  axis(a_limit(i, :)) ; 
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'/.  view (2); 

'/,  axis( ’square’ ) : 
y,  axis  (’off’); 
f o_hdl=arrow(  [0  0  0],... 

CF_IT((i-l)*3+l,l)  F_IT((i-l)*3+2,l)  F_IT((i-l)*3+3 , 1)] ) ; 
arrow (fo_hdl, ’BaseAngle’ ,45, ’LineWidth’ ,2, ’Color’ ,colors(68, :)) ; 

end 

for  j=2:numit 
for  i=l:n 

'/plot  contact  forces 

f igure(f_hdl(i+l))  :  '/make  figure  f_hdl(i)  the  current  figure 

y,  af _hdl(i)=axes ; 

y,  axis(a_limit(i, :)) : 

y.  view(2) ; 

y,  axis( ’square’ ) ; 

y,  axis  ( ’  off  ’ ) ; 

y,  hold  on; 

f color=bcolor+3*j ; 

df_hdl=arrow([F_IT((i-l)x-3+l,j-l)  F_IT((i-l)*3+2,  j-1)  .  .  . 

F_IT((i-l)*3+3,j-l)],... 

CF_IT((i-l)*3+l, j)  F_IT((i-l)*3+2, j)  . . . 

F_IT((i-l)*3+3,j)]); 

arrow(df _hdl, ’BaseAngle’ ,45, ’LineWidth’ ,2, ’Color’ , colors (f color, :)) ; 

if  e_flag==l  ft  max(abs(E_IT((i-l)*3+l: (i-l)*3+3,j-l)))>0 
'/.plot  contact  force  errors 
e_hdl=arrow([F_IT((i-l)*3+l,j-l) . . . 
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F_IT((i-l)*3+2, j-1)  F_IT((i-l)*3+3, j-1)] , . . . 
[(F_IT((i-l)*3+l. j-l)+E_IT((i-l)*3+l,j-l)) , • • • 
(F_IT((i-l)*3+2,j-l)+E_IT((i-l)*3+2,j-l)),. . . 
(F_IT((i-l)*3+3.j-l)+E_IT((i-l)*3+3,j-l))]); 
arrow (e_hdl, ’BaseAngle’ ,16, ’Color’ , ’w’) ; 
end 

'/.drawnow 

end 

end 


B.1.6  fp^plot. 


function  fp_plot(F_IT,numit) 
global  n 

'/.for  display  test  purposes 
*/,nuiiiit=l ; 

'/,  degrees  to  radians 
deg2rad=pi/180; 

'/.this  function  plots  the  contact  force  convergence  history 
'/.for  each  CFA  solution 

'/.numit  -  number  of  iterations  for  convergence 

'/.F_IT  -  matrix  of  contact  forces  applied  during  each  step  of  soln. 

'/.in  object  frame,  column  length  =  3*n,  row  length  =  numit 

'/.since  forces  are  in  local  contact  frame,  only  need  x  and  y  components 

'/.for  display  purposes. 
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i=l; 

figure 

axis([l  numit  min(  Cmiii(F_IT( (i-1) *3+1 , : )) 
min(F_IT( (i-l)*3+2, :))])- .5  ... 
max([max(F_IT((i-l)*3+l, :))  ... 
max(F_IT((i-l)*3+2. :))])+. 5  ]) 
hold  on 

p_hdl=  plot((l:numit) ,F_IT((i-l)*3+l,l:numit) , ’ro*) 

SGt(p_hdl, ’LineWidth’ ,2) :  */,set  line  width  to  2 
p_hdl=  plot((l:numit) ,F_IT((i-l)*3+2,l:numit) , ’g*’) 
set(p_hdl, ’LineWidth’ ,2) ;  '/set  line  width  to  2 
p_hdl=  plot (Cl  numit] , [-1 .25  -1.25], *w’) 
set(p_hdl, ’LineWidth’ ,2)  ;  ‘/set  line  width  to  2 
p_hdl=  plot (Cl  numit], C. 5  .5],’w’) 

SGt(p_hdl, ’LineWidth’ ,2) ;  '/set  line  width  to  2 

title(’FLRS  Two  Contact  Solution’) 
xlabeK ’ Iteration  No.’); 
ylabeK ’Contact  Force’); 
axis (’square’) ; 

legend(’ro’ , ’Fix’ , ’g*’ , ’Fly’) 

i=2; 

figure 

axis(Cl  numit  min(Cmin(F_IT((i-l)*3+l, :))  min(F_IT( (i-l)*3+2, :))])- .5  ... 
max(Cmax(F_IT((i-l)*3+l, :))  max(F_IT((i-l)*3+2, :))])+ .5  ]) 
hold  on 

p_hdl=  plot((l:numit) ,F_IT((i-l)*3+l,l:numit) , ’ro’) 
set(p_hdl, ’LineWidth’ ,2) ;  '/set  line  width  to  2 
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p_hdl=  plot((l:numit) ,F_IT((i-l)*3+2,l:numit) , ’g*’) 

set(p_hdl, ’LineWidth’ ,2) ;  '/.set  line  width  to  2 

p_hdl=  plot(Cl  numit] , [2 . 25  2.25], ’w*) 

set (p_hdl, ’LineWidth’ ,2) ;  '/set  line  width  to  2 

p_hdl=  plot([l  numit], [.5  .5],’w’) 

set (p_hdl, ’LineWidth’ ,2) ;  '/set  line  width  to  2 


title (’FLRS  Two  Contact  Solution’) 
xlabeK  ’  Iteration  No.’); 
ylabeK ’Contact  Force’); 
axis ( ’ square ’ ) ; 
legendC’ro’ ,’F2x’ ,’g*’ ,’F2y’) 


B-34 


Appendix  C.  Real-Time  Code 

The  following  C  code  is  the  FLRS  algorithm  incorporated  into  Chimera  com¬ 
pliant  modules.  The  module  cfaload.c  generates  the  commanded  object  wrenches 
and  records  the  time  elapsed  for  a  solution.  The  module  cfa.c  actually  solves  for  the 
contact  forces  given  the  object  wrench  and  grasp  configuration  data.  Functions  not 
included  include  standard  matrix  manipulation  routines  by  H.T.  Lau,  Numerical  Li¬ 
brary  in  C  for  Scientists  and  Engineers^  CRC  Press,  Inc.,  1995,  ISBN  0-8493-7376-X. 

C.l  cfaload.c 

/*  */ 

/*  cfaload.c  */ 


/*  */ 

/*  created  by  Mark  W  Hunter  12-13-95  */ 

/*  Air  Force  Institute  of  Technology  */ 

/*  */ 

/*  modified:  */ 

/*  */ 

/*  $  date  $  $  initials  $  $  comments  $  */ 

/*  */ 

/*  reviewed  by  $  Some  name  here  $  $  date  $  */ 

/*  */ 

/* - */ 

/*  */ 


/*  cfaload:  this  module  is  used  to  test  the  cfa  module  by  */ 

/*  sending  various  wrench  commands  and  sending  the  cfa  contact  */ 
/*  dforce  results  to  file  along  with  cpu  time  required  for  */ 

/*  solution.  */ 
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/*  */ 

/*  State  variable  table:  */ 

/*  INCONST:  none  */ 

/*  OUTCOST:  N.CONTACT  -  number  of  contacts  */ 

/*  MU  -  coeffecient  of  friction  between  */ 

/*  manipulators  and  object  surface  */ 

/*  */ 

/*  INVAR:  S_FLAG  -  signal  flag,  determines  */ 

/*  which  module  should  be  */ 

/*  currently  running  */ 

/*  F_CNT  -  contact  force  output  */ 

/*  from  "cfa"  */ 

/*  */ 

/*  OUTVAR:  WRENCH  -  commanded  object  wrench  */ 

/*  CNT.POS  -  contact  positions  */ 

/*  in  object  frame  */ 

/*  X_CNT  -  orientation  of  contact  x-axis*/ 

/*  (which  is  an  inward  pointing*/ 

/*  normal  to  surface)  in  object*/ 

/*  frame.  */ 

/*  Y_CNT  -  orientation  of  contact  y-axis*/ 

/*  (tangent  to  contact  surface)*/ 

/*  S_FLAG  -  signal  flag,  determines  */ 

/*  which  module  should  be  */ 

/*  currently  running  */ 

/*  */ 

/*  FC_0FFSET  -  friction  cone  offset  for  grasp  stability  */ 

/*  C0NV_0FFSET  -  solution  convergence  offset(.l  nominal)  */ 

/*  */ 

/*  */ 
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/*  Special  notes:  */ 

/*  */ 

/*  */ 

I  if.  ifififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififif  iff 

/*  include  files  */ 

/if  ifififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififif  if / 

tinclude  <chimera.h> 

#include  <sbs,h> 

#include  <string.h>  /*  string  functions  */ 
tinclude  <stdio.h>  /*  standard  input/output  library  */ 
tinclude  <stdarg.h> 
tinclude  <ctype.h> 

tinclude  <stdlib.h>  /*  standard  library  */ 
tinclude  <math.h>  /*  standard  math  library  */ 

/♦tinclude  <cfa.h>*/ 

!*  macro  definitions  */ 

tdefine  PI  3.141592654 
float  *temp; 

/*  module  ’Local_t’  definition  as  required  by  Chimera  */ 


typedef  struct  i 
char  f name  [80] ; 
char  title  [80] ; 
int  *n; 
float  *mu; 
float  *del_X; 
float  *fc_x; 
float  *Q;  /*Q[7]*/ 
int  iQ_ctr; 

float  *R_temp;  /*R_temp [16] */ 
float  *XC_temp ; /*XC_temp [16] */ 
float  *YC_temp;/*YC_temp[16]*/ 
float  *F_temp; 
int  *s_flag; 
svarVar.t  *s_flag_; 
float  *time; 

}  cf aloadLocal.t ; 

FILE  *rec; 


/*  module  initialization  as  required  by  Chimera  */ 

SBS_MODULE(cfaload); 
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/*  functions  */ 

/*  function  declarations  */ 


j  9fC  3|(3|(3|C3|C3|(9|(3f()f(9f(9{(3{C)|<)|<){C34e3|(}|(3K^^^^^^3t(3|(3)e9|(9|(9|t3t(3f(3tt9te3(C3|C3|C3|C)t(%)i()t(9t(9|(34(He3|C3|C3{C){C){(3{C)|C3jC9)(3l(9^3)(3l«9|<3lC3|(3|C}|C3|(9|(»|()|(  9f(  j 

/*  cfaloadinit  Initialize  the  module.  */ 

/*  Jit*************************!!'***!)'**********")'**************************  */ 

int  cfaloadinit (c info,  local,  stask) 
cfigInfo_t  *cinfo; 
cf aloadLocal_t  *local; 
sbsTask_t  *stask; 

sbsSvar.t  *svar  =  &stask->svar ; 


/*  Get  pointers  to  state  variables  (CONST) .  */ 


local->n  =  svarTranslateValue(svar->vartable,  "N.CONTACT",  int); 
local->mu  =  svarTranslateValue(svar->vartable,  "MU",  float); 

/*  Get  pointers  to  state  variables .  */ 

local->s_flag_  =  svarTranslate(svar->vartable,  "S_FLAG"); 

local->s_flag  =  svarValue(local->s_flag_ ,  int); 

local->F_temp  =  svarTranslateValue(svar->vartable,  "F_CNT",  float); 
local->time  =  svarTranslateValue(svar->vartable,  "CPU.TIME",  float); 
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local->Q  =  svarTranslateValue(svar->Vcirtable,  "WRENCH",  float); 
local->R_temp  =  svarTranslateValue(svar->vartable,  "CNT.POS",  float); 
local->XC_temp  =  svarTranslateValue(svar->vartable,  "X_CNT",  float); 
local->YC_temp  =  svarTranslateValue(svar->vartable,  "Y_CNT",  float); 

local->fc_x  =  svarTranslateValue(svar->vartable,  "FC.OFFSET",  float); 
local->del_X  =  svarTranslateValue(svar->vartable,  "CONV.OFFSET" ,  float); 


/*  define  OUTCONST  contact  parameters  */ 

local->n[0]=3;  /*  number  of  contacts  */ 
local->muC0]=.5;  /*  coefficient  of  friction*/ 


return  (int)  local; 

} 


/*  cf aloadReinit  Re-Initialize  the  module.  */ 

int  cf aloadReinit (local,  stask) 
cf aloadLocal_t  *local; 
sbsTask_t  *stask; 

{ 

return  I_0K; 

> 
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/*  cfaloadOn  Start  up  the  module.  */ 

int  cf aloadOnClocal,  stask) 
cf aloadLocal_t  *local; 
sbsTask_t  *stask; 

{ 

kprintf ("cf aload:  0N\n"); 

printf ("Starting  the  Contact  Force  Assignment  Test  Module  \n"); 
/*  initialize  signal  flag  */ 


/*  explicit  write  to  state  variable  table  */ 

local->s_f lag [0] =2 ; 
svarWrite(local->s_flag_) ; 

/*  define  contact  parameters  */ 
local->del_XCO]=.l;  /*  convergence  buffer  */ 
local->f c_x  [0] =0 ;  /*  stability  buffer  */ 

/*  define  contact  postions  */ 
local->R_temp [0] =0 ; 

local->R_temp [1] =1 ; 
local->R_temp [2] =0 ; 
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local->R_temp [3] =0 ; 


local->R_temp [4]  =0 ; 
local->R_temp [5] =-l ; 
local->R_temp [6] =0 ; 

local->R_temp [7] =-l ; 
local->R_temp [8] =0 ; 
local->R_temp [9] =0 ; 


/*  define  local  x-axis  in  object  frame  */ 
local->XC_temp [0] =0 ; 

local->XC_temp [1] =- 1 ; 
local->XC_temp [2] =0 ; 
local->XC_temp [3] =0 ; 

local->XC_temp [4] =0 ; 
local->XC_temp [5] =1 ; 
local->XC_temp [6] =0 ; 

local->XC_temp [7] =1 ; 
local->XC_temp [8] =0 ; 
local->XC_temp [9] =0 ; 

/*  define  local  y-axis  in  object  frame  */ 
local->YC_temp [0] =0 ; 
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local->YC_temp [1] =0.0; 
local->YC_temp [2] =-l . 0 ; 
local->YC_temp [3] =0.0; 

local->YC_temp [4] =-l .0; 
local->YC_temp [5] =0.0; 
local->YC_temp [6] =0.0; 

local->YC_temp [7] =0 ; 
local->YC_temp [8] =1 ; 
local->YC_temp [9] =0 ; 

/*  initalize  wrench  values  */ 
local->Q[0]=0; 

local->Q[l]=0; 
local->q[2]=0; 
local->Q[3]=0; 
local->Q [4]=0; 
local->q [5]=0; 
local->q [6] =0 ; 

local->iq_ctr=0 ; 

kprintfC'The  current  value  of  s-flag  should 
be  2,  s_flag=  */,d  \n",  local->s_flag[0] ) ; 

/*  open  a  file  to  print  results  of  test  */ 

if  ((rec  =  f open( "TEST. DAT", "wt"))  ==  NULL) 
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{ 

kprintfC'Can  not  create  ’*/,s  ’  \n"  ,  "TEST.DAT")  ; 

> 


return  I_0K; 

> 

/if  ifififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififififif  if/ 

/*  cf aloadCycle  Process  module  information.  */ 

/if  ififif»fififififtfifififififififif^ilf1f:^1f»f^t:»fif^ififififififilf:>fifififif)iiif}»fifififififififififififififififififififififififififif  if/ 

\ 

int  cf aloadCycle (local,  stask) 
cf aloadLocal.t  *local; 
sbsTask.t  *stask; 

-C 

if  (local->s_flag[0]==2)  /*if  s_flag==2,  time  to  generate  Q,*/ 

/*  if  s_flag==l,  ignore  this  cycle  */ 

{ 

fprintf (rec,"'/,d  \t  '/,f 

\t  '/.f  \t  y.f  \t  y.f 
\t  y,f  \t  y.f  \t  y.f 
\t  y.f  \t  y.f  \t  y.f 
\t  y.f  \t  y.f  \t  y.f 
\t  y.f  \t  y.f  \t  y.f  \n", 

local->iQ_ctr ,  local->time [0] , 
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local->F_temp[l3 , 
local->F_temp[4] , 
local->F_temp[7] , 
local->F_temp[103 , 
local->F_temp[13] , 


local->F_temp[2] ,  local->F_temp[3] , 
local->F_temp[5] ,  local->F_tempC6] , 
local->F_temp [8] ,  local->F_temp [9] , 
local->F_tempCll] ,  local->F_temp [12] , 
local->F_temp  [14] ,  local->F_temp  [15] ) ; 


kprintf  ("This  is  the  current  value  of  theta,  */,d  \n",  local->iQ_ctr+l) ; 

local->iq_ctr=local->iq_ctr+l ; 
local->q [1] =cos (local->iq_ctr*PI/36) ; 
local->q [2] =sin(local->iq_ctr*PI/36) ; 


local->s_flag[0]=l;  /*  update  S.FLAG  to  1,  so  that  */ 

/*  cfa  will  execute  next  cycle  */ 

kprintf (  "The  current  value  of  s_flag  should  be  1, 
s_flag=  '/,d  \n"  ,local->s_flag[0]  )  ; 

if  (local->iQ_ctr>72)/*  finished  with  the  input  set,  end  test  */ 

local->s_f lag [0] =3 ; 

kprintf (  "The  input  set  is  now  finished,  kill  this  process  \n"); 

} 

} 


C-11 


return  I_OK; 

} 

/*  cfaloadOff  Stop  the  module.  */ 

int  cfaloadOff (local,  stask) 
cf aloadLocal_t  *local; 
sbsTask.t  *stask; 

kprintf ("cf aload:  0FF\n"); 

printf ("Stopping  the  Contact  Force  Assignment  Test  Module  \n"); 

f close(rec) ; 

return  I_0K; 

} 

/*  cfaloadKill  Clean  up  after  the  taodule.  */ 

int  cfaloadKill (local,  stask) 
cfaloadLocal.t  >Klocal; 
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sbsTask_t  *stask; 

{ 

kprintf ("cf aload:  FINISHED\n") ; 
return  I_0K; 

} 

/i|c  *********11! ***********  */ 

/*  cfaloadError  Attempt  automatic  error  recovery.  */ 

/*  ********************************************************************  *! 

int  cfaloadError (local,  stask,  mptr,  errmsg,  errcode) 

cfaloadLocal_t  *local; 

sbsTask.t  *stask; 

errModule.t  *mptr; 

char  * errmsg; 

int  errcode; 

{ 

/*  Return  after  not  correcting  error.  */ 

return  SBS .ERROR; 

} 

/*  ***************************************************************i^^^^if 

/*  cfaloadClear  Clear  error  state  of  the  module.  */ 

/*  **************************************************************jfififi^if^ 

int  cfaloadClear (local ,  stask,  mptr,  errmsg,  errcode) 
cf aloadLocal.t  >i<local; 
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sbsTask_t  *stask; 
errModule.t  *mptr; 
char  *errmsg; 
int  errcode; 

{ 

/*  Return  after  not  clearing  error.  */ 

sbsNewError(stask,  "Clear  not  defined,  still  in  error  state",  errcode): 
return  SBS_ERR0R; 

} 

/*  cfaloadSet  Set  module  parameters.  */ 

int  cfaloadSet (local,  stask) 
cf aloadLocal.t  *local; 
sbsTask.t  *stask; 

{ 

return  I_0K; 

} 

/*  cfaloadGet  Get  module  parameters.  */ 

int  cfaloadGet (local,  stask) 
cf aloadLocal_t  *local; 
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sbsTask_t  *stask; 

{ 

return  I_OK; 

} 

/*  cfaloadSync  For  modules  which  synchronize  to*/ 

/*  something  other  than  the  clock.  */ 

/Hf  ^|i^^|i^|i!l^:^^^|iif■ififilL■^^■^^^^^^il^■^ll^^l^i^*i^***^^^^**i^**■»^■»^■¥**********************************  */ 

int  cfaloadSync (local,  stask) 
cf aloadLocal_t  *local: 
sbsTask.t  *stask; 

{ 

return  I_0K: 

> 

C.2  cfa.c 

/*  */ 

/*  cfa.c  */ 

/*  */ 

/*  created  by  Mark  W  Hunter  12-05-95  */ 

/*  Air  Force  Institute  of  Technology  */ 

/*  */ 

/*  modified:  */ 

/*  */ 
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/*  $  date  $  $  initials  $  $  comments  $  */ 

/*  */ 

/*  reviewed  by  $  Some  name  here  $  $  date  $  */ 

/*  */ 

/if. - if/ 

/*  */ 


/*  cfaload:  this  module  is  used  to  test  the  cfa  module  by  */ 

/*  sending  various  wrench  commands  and  sending  the  cfa  contact  */ 
/*  dforce  results  to  file  along  with  cpu  time  required  for  */ 

/*  solution.  */ 

/*  */ 

/*  State  variable  table:  */ 

/*  INCONST:  N_C0NTACT  -  number  of  contacts  */ 

/*  MU  -  coeffecient  of  friction  between  */ 

/*  manipulators  and  object  surface  */ 

/*  OUTCONST:  none  */ 

/*  */ 

/*  INVAR:  WRENCH  -  commanded  object  wrench  */ 

/*  CNT_P0S  -  contact  positions  */ 

/*  in  object  frame  */ 

/*  X_CNT  -  orientation  of  contact  x-axis*/ 

/*  (which  is  an  inward  pointing*/ 

/*  normal  to  surface)  in  object*/ 

/*  frame.  */ 

/*  Y_CNT  -  orientation  of  contact  y-axis*/ 

/*  (tangent  to  contact  surface)*/ 

/*  S_FLAG  -  signal  flag,  determines  */ 

/*  which  module  should  be  */ 

/*  currently  running  */ 

/*  */ 
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/*  FC.OFFSET  -  friction  cone  offset  for  grasp  stability  */ 

/*  CONV.OFFSET  -  solution  convergence  offset(.l  nominal)  */ 

/*  */ 

/*  OUTVAR:  F_CNT  -  contact  forces  necessary  to  */ 

/*  generate  the  required  object*/ 

/ *  wrench  */ 

/*  S_FLAG  -  signal  flag,  determines  */ 

/*  which  module  should  be  */ 

/*  currently  running  */ 

/*  */ 

/*  Special  notes:  */ 

/*  */ 

/*  */ 

,|t%,|ti|ci|c,tciti*************************************************************  */ 

,ti,ti,|i,|i,|c,|c,tc*************************************************************  */ 

/*  include  files  */ 

/*  ,|i*******************************************************************  */ 

#include  <chimera.h> 

#include  <sbs.h> 

#include  <string.h>  /*  string  functions  */ 

#include  <stdio.h>  /*  standard  input/output  library  */ 

#include  <stdarg.h> 

#include  <ctype.h> 

#include  <stdlib.h>  /*  standard  library  */ 

# include  <math.h>  /*  standard  math  library  */ 

/*#include  <cfa.h>*/ 


C-17 


/*  macro  definitions  */ 

#define  PI  3.141592654 

/*  module  ’Local_t’  definition  as  required  by  Chimera  */ 

typedef  struct  { 

char  fname[80];  /*  file  name  for  output  data  */ 

char  title  [80];  /*  title  of  output  vector  or  matrix  */ 

int  *n;  /*  number  of  contacts  (assume  5  is  the  max  number)  */ 

int  i3n,inn,num_I; 

float  *mu;  /*  coeffecient  of  friction  */ 

float  *del_X;  /*  convergence  buffer  for  fuzzy  solver  */ 
float  *fc_x;  /*  friction  cone  offset,  for  contact  stability  */ 

/*  rulebase  for  fuzzy  logic  system  7x7  plus  zero’th  row  and  col  offsets  */ 
int  **RB_ef; 

/*  fuzzy  domain  for  dot  product  information  7x1  plus  zero’th  element*/ 
float  *c_dot; 

/*  fuzzy  range  of  e_I  effectiveness  7x1  plus  zero’th  element  */ 
float  *cfd_wt; 

/*  area  of  fuzzy  membership  functions  of  cfd_wt  7x1  */ 
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float  *afd_wt; 


/*  number  of  membership  functions  which  comprise  the  domain  of  inputs  */ 

/*  to  the  fuzzy  analysis  portion  of  this  code  */ 
int  nc; 

/*  input  contact  positions  as  col.  vectors,  allow  up  to  five  contacts  */ 
float  **r; 

/*  xcC]C]  and  yc[][]  are  inputs  of  contact  orientation  */ 

/*  *c  is  the  contact  *-axis  direction  in  object  frame  */ 
float  **xc,  **yc,  **zc; 

float  **Fo;  /*  external  contact  force  solution,  3xn  */ 

float  *q;  /*  object  wrench  6x1  plus  zero’th  element  */ 

float  **F;  /*  matrix  of  contact  forces  3xn  */ 

float  **T_i;  /*  [3+1]  [d3nl]  ;/*  trcinsf ormation  matrix  3x3n  */ 

float  **e_I;/*[3+l] [dnnl] ;/*  internal  force  unit  vectors,  Sxn+n  */ 

float  **e_Ic;/*[3+l]  [dnnl]  ;/>t'e_I  in  local  coordinates  for  each  contact  */ 

float  **W;  /*[6+l] [d3nl] ;/*  grasp  matrix  */ 

float  **Wp;  /* [6+1] [d3nl] ;/*  psuedoinverse  of  grasp  matrix  */ 


/*  below  are  arrays  used  in  ’h_sol’  */ 
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float  **F_i_old,  **Fc,  *theta,  *fc_check,  **ER_f,  **ER_fc; 
float  *vl_l; 

float  *ER_dot,  *wt_tot; 

float  **mu_ERd,  **wt_ef,  *nER_f,  **d_F,  **d_Fc: 

float  *d; 

int  **imf_ERd; 

/*  these  variables  are  used  for  temporary  storage  of  state  varibles  which  */ 

/*  are  in  vector  form  and  must  be  converted  to  matrix  form  */ 

float  *F_temp,  *R_temp,  *XC_temp,  *YC_temp; 

float  *time; 

int  *s_flag; 

}  cfaLocal_t; 

/*  module  initialization  as  required  by  Chimera  */ 

SBS_MODULE(cfa) ; 

/*  fiinctions  */ 

/*  function  declarations  */ 

/*  unless  otherwise  noted,  these  functions  are  authored  by:  */ 

/*  H.T.  Lau  */ 

/*  Numerical  Library  in  C  for  Scientists  and  Engineers  */ 

/*  1995  CRC  Press,  Inc.  */ 

/*  ISBN  0-8493-7376-X  */ 
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void  inivec(int,  int,  float  [] ,  float); 

/*  initializes  a  real  vector  to  a  given  value  */ 
void  inimat(int,  int,  int,  int,  float  **,  float); 

/*  initializes  a  real  matrix  */ 

void  ini_imat(int,  int,  int,  int,  int  **,  int); 

/*  initializes  an  int  matri  */ 

float  matvec(int,  int,  int,  float  **,  float  []); 

/*  s=mat(row)*vec  */ 

float  tamvec(int,  int,  int,  float  **,  float  [] ) ; 

/*  s=mat(col)*vec  */ 

float  matmat(int,  int,  int,  int,  float  **,  float  **); 
void  fulmatvec(int ,  int,  int,  int,  float  **,  float  [],  float  []); 
/*  c[]=mat*vec  */ 

void  fultamvec(int,  int,  int,  int,  float  **,  float  [],  float  []); 
/*  c[]=mat(col)*vec  */ 

void  dupmatCint,  int,  int,  int,  float  **,  float  **) ; 

/*  copy  b[][]  into  a[]  []  */ 

float  vecvec(int,  int,  int,  float  [],  float  []); 

/*  inner  vector  product  */ 


float  **allocate_real_matrix(int,  int,  int,  int); 
int  **allocate_int_matrix(int ,  int,  int,  int); 
float  *allocate_real_vector(int,  int); 
void  free_real_matrix (float  **,  int,  int,  int); 
void  free_ int .matrix (int  **,  int,  int,  int); 
void  free_real_vector (float  *,  int); 

int  psdinv (float  **,  int,  int,  float  []);/*  psuedo  inverse  solution  */ 
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int  qrisngvaldec (float  int,  int,  float  [],  float  float  []); 
void  hshroabid (float  **,  int,  int,  float  [] ,  float  [],  float  []); 
float  tammatdnt,  int,  int,  int,  float  **,  float  **) ; 
float  mattam(int,  int,  int,  int,  float  **,  float  **) ; 
void  elmcoKint,  int,  int,  int,  float  **,  float  **,  float); 
void  elnirow(int,  int,  int,  int,  float  **,  float  **,  float); 
void  psttfmmat (float  **,  int,  float  **,  float  []); 

/*  matmat,  elmcol  */ 

void  pretfmmat (float  **,  int,  int,  float  []); 

/*  tammat,  elmcol  */ 

int  qrisngvaldecbid (float  [],  float  [],  int,  int,  float  **,  float  **, 
float  □ ) ; 

void  rotcoKint,  int,  int,  int,  float  **,  float,  float); 

void  psdinvsvd(float  **,  float  □,  float  **,  int,  int,  float  □); 

/♦matvec*/ 

void  system. error (char  []); 

/*  euclidean  norm  of  vector  (mhunter)  */ 
float  euclnormdnt,  int,  float  []); 

/*  cross  product  of  two  spatial  (x,y,z)  input  vectors  (mhunter)  */ 
void  cross  (int,  int,  float  [],  float  [],  float  []); 

/*  print  out  real  matrix  to  file  (mhunter)  */ 

void  m_out (float  **,  int,  int,  int,  int,  char[],  char[]); 

/*  print  out  int  matrix  to  file  (mhunter)  */ 

void  im_out(int  **,  int,  int,  int,  int,  char[]  ,  chard); 
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/*  print  out  real  vector  to  file  (mhunter)  */ 
void  v_out (float  []  ,  int,  int,  charC]  ,  charD); 

/*  print  out  real  scalar  to  file  (mhunter)  */ 
void  s.out (float,  char[],  char[]); 


/*  solve  for  the  contact  forces  given  the  object  wrench  (mhunter)  */ 
void  h_sol(  float,  float,  float,  int,  int,  int, 
int  **,  float  []  ,  int,  float  [],  float  [], 
float  **,  float  **,  float  **, 
float  **,  float  **,  float  [] ,  float  **, 
float  **,  float  **,  float  [], 
float  [],  float  **,  float  **,  float  □, 
float  [] ,  float  [] ,  int  **,  float  **,  float  **, 
float  []  ,  float  **,  float  **,  float  []); 

/*  determine  maximum  value  from  given  vector  (mhunter)  */ 
float  m3rmax(int,  int,  float  []); 

/*  determine  maximum  value  from  given  vector  (mhunter)  */ 
float  mymin(int,  int,  float  []); 

/*  fuzzy  inference  mechanism  (mhunter)  */ 

void  mem_edci(  int  **,  float  **,  float  [],  int,  float  [] ,  int); 


/*  *  iK  Xc  ><<  >t<  ^  >|!  >l<  >|(  >|(  >|(  ’ll  >|t  >l<  !(<  I|<  I|< >)< ’ll* Hi  It!  Ik  ik  / 

/*  cfalnit  Initialize  the  module.  */ 

y  9k  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  >k  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  Ik  !k  Ik  !k  Ik  Ik  ik  / 
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int  cfaInit(cinfo,  local,  stask) 
cfiginfo.t  *cinfo; 
cfaLocal_t  *local; 
sbsTask.t  *stask; 

{ 

sbsSvar_t  *svar  =  &stask->svar ; 

/*  Get  pointers  to  state  variables  (CONST) .  */ 

local->n  =  svarTranslateValue(svar->vartable,  "N_C0NTACT",  int); 
local->mu  =  svarTranslateValue(svar->vartable,  "MU",  float); 
local->s_f lag  =  svarTranslateValue(svar->vartable,  "S_FLAG",  int); 

local->Q  =  svarTranslateValue(svar->vartable,  "WRENCH",  float); 
local->R_temp  =  svarTranslateValue(svar->vartable,  "CNT.POS",  float); 
local->XC_temp  =  svarTranslateValue(svar->vartable,  "X_CNT",  float); 
local->YC_temp  =  svarTranslateValue(svar->vartable,  "Y_CNT",  float); 
local->fc_x  =  svarTranslateValue(svar->vartable,  "FC.OFFSET",  float); 
local->del_X  =  svarTranslateValue(svar->vartable,  "CONV.OFFSET" ,  float); 
local->F_temp  =  svarTranslateValue(svar->vartable,  "F_CNT",  float); 

local->time  =  svarTranslateValue(svar->vartable,  "CPU.TIME",  float); 


return  (int)  local; 

> 

/*  cfaReinit  Re-Initialize  the  module.  */ 
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int  cfaReinit (local,  stask) 
cfaLocal_t  *local; 
sbsTask.t  *stask; 

{ 

return  I_0K; 

> 

/*  cfaOn  Start  up  the  module.  */ 

j  if  :)<  i|:  i|(  i|(  i|c  ^  i|i  i|t  ^  ifi  !f<  lit  i|<  It!  If:  ^ ’ll  ’ll  lit  / 

int  cfaOnClocal,  stask) 
cfaLocal.t  *local; 
sbsTask.t  *stask; 

{ 

float  vl[4]=  {0,0, 0,0}; 
float  v2[4]=  {0,0, 0,0}; 
float  v3[4]=  {0, 0,0,0}; 
float  r_ij[4]=  {0,0, 0,0}; 
float  rij[4]=  {0,0, 0,0}; 
float  r_normC4]={0,0,0,0}; 

float  I3C4][4]=  {  {0,  0,  0,  0}, 

{0,  1,  0,  0}, 

{0,  0,  1,  0}, 

{0,  0,  0,  1}  }; 
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float  PiC4]  [4]={  {0,0, 0,0}, 

{0,0, 0,0}, 

{0,0, 0,0}, 

{0,0, 0,0}  }; 

/*  emC]  controls  exit  convergence  criteria  on  psuedoinverse  solution  */ 
float  emC8]={1.0e-14,  0.0,  l.Oe-12,  0.0,  40.0,  0.0,  l.Oe-10,  0.0}; 

int  i,j,jj,k;  /*  counters  *f 


kprintf ("cf a:  0N\n"); 

kprintf ("Starting  the  Contact  Force  Assignment  Module  \n"): 
local->nc®7;/*  number  of  fuzzy  membership  functions  across  input  domains  */ 
/*  Get  pointers  to  state  variables.  */ 

/*  local->q=allocate_real_vector(l ,6) ; 

local->R_temp=allocate_real_vector (1 , 15) ; 
local->XC_temp=allocate_real_vector (1 , 15) ; 
local->YC_temp=allocate_real_vector(l ,15) ; 
local->F_temp=allocate_real_vector(l,15) ;  */ 

/*  Allocate  memory  and  intialize  variables  */ 
local->i3n=3.0  *  (local->n[0] ) ; 
local->inn=local->n[0]  *  (local->nC0] ) ; 

local->num_l=(local->n[0]  *  (local->n[0] )  -  local->n[0] )/2: 

/*  number  of  internal  forces  */ 
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local->RB_ef =allocate_int_matrix(l ,7 , 1,7); 
local->c_dot=allocate_real_vector(l,7) ; 
local->cfd_wt=allocate_real_vector(l,7) ; 
local->afd_wt=allocate_real_VGCtor (1 ,7) ; 
local->r=allocate_real_matrix(l ,3, 1 ,local->n [0] ) ; 
local->xc=allocate_real_matrix (1,3, 1 ,local->n [0] ) ; 
local->yc=allocat e_real_matr ix (1,3,1, local->n [0] ) ; 
local->zc=allocate_rGal_matrix(l ,3, 1 ,local->n[0] ) ; 
local->Fo=allocate_r6al .matrix (1,3, 1 ,local->n [0] ) ; 
local->F=allocate_real_matrix(l ,3, 1 ,local->n [0] ) ; 
local->T_i=allocate_real_matrix(l ,3 , 1 ,local->i3n) ; 
local->e_I=allocate_rGal_matrix(l,3,l,local->iim) ; 
local->G_Ic=allocatG_rQal_matrix(l ,3,l,local->inn) ; 
local->W=allocatG_rGal_matrix (1,6,1, local->i3n) ; 
local->Wp=allocatG_rGal_matrix(l ,local->i3n, 1,6) ; 


local->F_i_old=allocatG_rGal_matrix(l ,3,1 ,local->nC0] ) ; 
local->Fc=allocatG_rGal_matrix(l ,3 , 1 ,local->n[0] ) ; 
local->thGta=allocatG_rGal_VGCtor(l,  local->n[0]) ; 
local->fc_chGck=allocatG_rGal_VGCtor(l,  local->nC0]) ; 
local->ER_f=allocatG_rGal .matrix (1,3,1, local->n [0] ) ; 
local->ER.f c=allocatG.rGal.matrix(l,3, l,local->n[0] ) ; 
local->vl.l=allocatG.rGal.vGctor(l,  3) ; 
local->ER.dot=allocatG.rGal.vGctor(l,  local->inn) ; 
local->wt.tot=allocatG.rGal.vGctor(l,  local->inn) ; 
local->nER.f =allocatG.rGal.VGCtor ( 1 ,  local->n  [0] ) ; 
local->imf .ERd=allocatG.int.matrix(l ,local->inn,l ,2) ; 
local->mu.ERd=allocatG.rGal.matrix(l,local->iim,l ,2) ; 
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local->wt_ef=allocate_real_matrix(l,local->nc,  1,  local->num_I) ; 
local->d_F=allocate_real_matrix(l ,3 , 1 ,local->nCO] ) ; 
local->d_Fc=allocate_real .matrix (1,3,1, local->n [0] ) ; 
local->d=allocate_real_vector(l ,  local->inn) ; 

/*  Reformat  *_temp  vectors  is  matrix  format  as  noted  below  in  the  example  */ 
f or(i=l ; i<=local->n[0] ; i++) 
for(j=l; j<=3; j++) 

local->rCj] Ci]=local->R_temp[(i-l)*  (local->n[0] )+j] ; 
local->xc[j] [i] =local->XC_temp [(i-1)*  (local->nC0] )+j] ; 
local->ycCj] [i]=local->YC_temp[(i-l)*  (local->n[03 )+j] ; 

} 

} 


/*  example  of  what  input  matrices  should  look  like  */ 


/*  r[l3  [1]=1;  rCl]  [2]  = 


r[l]C3]=-l;  r[l][4]=0 


r[l][5]=0;  */ 


/* 

r[2]  [1]=0; 

r[2][2]=-l;  r 

[2]  [33=0;  1 

■[23  [43=0;  r  [23  [53=0;  */ 

/* 

r[3]  [1]=0; 

r  [3]  [2]  =  0 ;  r 

[33  [33=0;  r  [33  [43=0;  r  [33  [53=0;  */ 

/* 

*! 

/* 

xc[l]  Cl]=-1 

;  xc[l]  [2]=  0; 

xc[l3  [33=1 

xc[l3  [43=0; 

xc[l3  [53=0; 

*/ 

/* 

xc[2]  [1]=0; 

xcC2]  C2]=l; 

xc [23 [33  =0 

xc [23 [43  =0 ; 

xc[23  [53=0; 

*/ 

/* 

xcC3]  [1]=0; 

xc  [3]  [2]  =  0 ; 

xc [33 [33  =0 

xc [33 [43  =0 ; 

xc[33  [53=0; 

*/ 

/* 

*/ 

/* 

o 

II 

1 — 1 

1 _ 1 

1 — 1 

l_l 

o 

>» 

yc[l]  C2]=-l; 

yc[i3  [33=0 

yc[l3  [43=0; 

yc[l3  [53=0; 

*/ 

/* 

ycC2]  Cl]=-i 

;  ycC2][2]=0; 

yc[23  [33=1 

yc [23 [43  =0 ; 

yc[23  [53=0; 

*/ 

/* 

ycC3]  Cl]=0; 

yc  [3]  [2]  =  0 ; 

yc[33  [33=0 

yc [33 [43  =0 ; 

yc[33  [53=0;*/ 
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/*  calculate  zc  using  cross  product  of  xc  and  yc  */ 


for(j=l;  j<=local->n[0] ;  j++) 

-C 

for(i=l;  i<=local->nC0] :  i++) 

vlCi]=local->xc[i]  [j]  ; 
v2[i]=local->yc[i]  [j]  ; 

} 

cross (1,  3,  vl,  v2,  v3) ; 

for(i=l; i<=3; i++) 

{ 

local->zc[i]  [j]=v3[i]  ; 

} 

} 

for(i=l;  i<=local->n[0] ;  i++)  /*  form  coordinate  transformation  matrices  */ 

for(k=l ;k<=3;k++) 

■c 

local->T_i Ck] [3*(i-l)+l]=local->xc[k] [i]  ; 
local->T_i[k] [3*(i-l)+2]=local->yc[k] [i] ; 
local->T_i[k] C3*(i-l)+3]=local->zc[k3 [i] ; 

} 

} 

for(i=l;  i<=local->n[03 ;  i++)  /*  form  internal  force  vectors  */ 

{ 

for(j=l;  j<=local->nC0] ;  j++) 
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{ 

j j=(i-l)*local->nCO]+j ; 


if(i==j) 

for (k=l ; k<=3 ; k++) 


local->e_I [k] Cjj]=0; 

> 

} 


else 

for(k=l ;k<=3;k++) 

•C 

r_ij  [k]=local->r[k]  [i] -local->r[k]  [j]  ; 
rij  Ck]=local->rCk] [i] -local->r [k]  [j]  ; 

> 


r_norm[j j]=euclnorm(l,3,rij) ; 


for(k=l ;k<=3;k++) 


local->e_I  [k]  Cjj]=-r_ij  [k]/r_norni[j  j]  ; 

> 

} 

} 

> 


/*  in  order  to  calculate  e_Ic,  which  is  the  internal  */ 
/♦force  vectors  in  the  */ 
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/♦frame  local  to  themselves,  vectors  vl  and  v2,  and  */ 
/♦matrix  zc  will  be  over  written  ♦/ 

for(i=l;  i<=local->n[0] ;  i++) 

{ 

for(j=l:  j<=local->n[0] ;  j++) 

■C 

j j=(i-l)^local->nCO]+j ; 

for(k=l:k<=3;k++) 

■c 

vlCk]=local->e_I[k]  [j  j]  ; 
local->zcCk] [1]=  local->T_i[k] [3^(i-l)+l3 ; 
local->zc[k] [2]=  local->T_i[k] [3^(i-l)+23 ; 
local->zc[k3 [33=  local->T_i[k3  [3^(i-l)+33 ; 

> 

f ultamvec ( 1 , 3 , 1 , 3 , local->zc , vl , v2) ; 

/♦  the  returned  vector  v2,  is  the  result  ♦/ 

/♦of  transpose  of  transformation  matrix  ♦/ 

/♦  multiplied  by  the  global  e_I  vectors  ♦/ 

/♦  associated  with  each  contact.  ♦/ 
for(k=l ;k<=3;k++) 

{ 

local->e_IcCk3  [jj3=v2[k3  ; 

> 

> 

}  /♦  end  internal  force  vector  formulation  ♦/ 
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/*  begin  grasp  matrix  formulation  */ 


f or (i=l ; i<=local->nC0] ; i++) 

{ 

Pi  Cl]  C2]=-local->r  [3]  [i] ; 
PiCl]C3]=  local->r  [2]  [i]  ; 
Pi[2][l]=  local->r  [3]  [i]  ; 

Pi  [2]  C3]=-local->r[l]  [i]  ; 

Pi [3] Cl]=-local->r [2] [i] ; 

Pi  [3]  [2]=  local->r  [1]  [i]  ; 


for(j=l; j<=3; j++) 

{ 

for(k=l ;k<=3;k++) 

{ 

local->WCk] [(i-l)*3+j]=I3[k] [j] ; 

} 

for(k=4;k<=6;k++) 

local->W[k] C(i-l)*3+j]=Pi[k-3]  [j]  ; 

> 

} 

}  /*  end  grasp  matrix  formulation  */ 

f or (i=l ; i<=local->i3n; i++) 

{ 

for(j=l; j<=6; j++) 
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{ 

local->Wp[i]  [j]=local->W[j]  [i]  ; 

} 

> 

i=psdinv (local->Wp , local->i3n , 6 , em) ; 


/*  initialize  fuzzy  rulebase  and  domain  definitions  */ 


local->RB_ef [1]  [1]  =  1; 
local->RB_ef  [1]  [3]=1; 
local->RB_ef [1] [53=2; 
local->RB_ef [1] [73=4; 
local->RB_ef  [23  [13  =  1; 
local->RB_ef  [23 [33  =2 ; 
local->RB.ef [23 [53=3; 
local->RB_ef [23 [73=5; 
local->RB_ef [33 [l3=l; 
local->RB_ef [33 [33=4; 
local->RB_ef [33 [53=4; 
local->RB_ef [33  [73=6; 
local->RB_ef [43 [l3=2; 
local->RB_ef [43 [33=4; 
local->RB_ef [43 [53=4; 
local->RB_ef [43 [73=6; 
local->RB_ef [53 [13=2; 
local->RB_ef [53 [33=4; 
local->RB_ef [53 [53=4; 


local->RB_ef [13 [23=  1; 
local->RB_ef [13 [43=2; 
local->RB_ef [13 [63=3; 

local->RB_ef [23  [23=  2; 
local->RB_ef [23 [43=3; 
local->RB.ef [23 [63=4; 

local->RB_ef [33 [23=  2; 
local->RB_ef [33 [43=4; 
local->RB_ef [33 [63=5; 

local->RB_ef [43  [23=  3; 
local->RB_ef [43 [43=4; 
local->RB_ef [43 [63=5; 

local->RB_ef [53 [23=  3; 
local->RB_ef [53 [43=4; 
local->RB_ef [53 [63=6; 
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local->RB_ef [5] [7]  =7; 

local->RB_ef [6] Cl]=3;  local->RB_ef [6] [2]=  4; 
local->RB_ef  [6]  [3] =5 ;  local->RB_ef [6] [4] =5 ; 
local->RB_ef  [6]  [5] =6;  local->RB_ef  [6] [6]=6; 
local->RB_ef [6] [7] =7; 

local->RB_ef [7] [1]=4;  local->RB_ef [7] [2]=  5; 
local->RB_ef [7] [3] =6 ;  local->RB_ef [7] [4] =6 ; 
local->RB_ef [7]  [5] =7 ;  local->RB_ef [7] [6] =7 ; 
local->RB_Gf [7] [7] =7; 

/*  fuzzy  domain  for  dot  product  information  7x1  plus  zero’th  element*/ 

local->c_dot [1] =-l ; 

local->c_dot [2] =- . 6 ; 

local->c_dot [3] =-.2; 

local->c_dot [4] =0 ; 

local->c_dot [5]  = . 2 ; 

local->c_dot [6] = . 6 ; 

local->c_dot [7] =1 ; 

/*  fuzzy  range  of  e_I  effectiveness  7x1  plus  zero’th  element  */ 

local->cf d_wt [1] =-l ; 

local->cf d_wt [2] . 5 ; 

local->cf d_wt [3] =-.2; 

local->cf d_wt [4] =0 ; 

local->cf d_wt [5] = . 2 ; 

local->cf d_wt [6] = . 5 : 

local->cf d_wt [7] =1 ; 

f or (i=l ; i<=local->nc ; i++) 
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/*  calculate  area  of  membership  functions  along  cfd_wt  */ 

{ 

if (i==l)  local->afd_wt  [i]=f abs(local->cfd_wt [i+l] -local->cfd_wt [i] )/2; 
else  if (i==local->nc-l)  local->afd_wt[i]= 
f abs (local->cf d_wt [i] -local->cf d_wt [i-1] ) /2 ; 

else  local->afd_wt  Ci]  =  (f abs(local->cfd_wt [i] -local->cfd_wt [i-1] )  + 
f abs (local->cf d_wt  [i+l] -local->cf d_wt [i] ) ) /2 ; 

} 

/*  print  out  various  vectors  and  matrices  for  checking  purposes  */ 

/*  print  out  variables  to  file  fname  */ 
strcpy (local->fname , "cf a_out") ; 

strcpy(local->title, "Local  X-Axes  in  Object  Frame,  xc  \n"); 
ni_out(local->xc,  1,  3,  1,  3,  local->title,  local->fname) ; 

/*  print  out  variables  to  file  fname  */ 

/ *strcpy (local->f name , " cf a_out " ) ; */ 

/*strcpy(local->title, "Local  Y-Axes  in  Object  Frame,  yc  \n");*/ 
/*m_out(local->yc,  1,  3,  1,  3,  local->title,  local->fname) ; */ 

/♦lit**********************************/ 

/*  print  out  variables  to  file  fname  */ 
/*strcpy(local->fname,"cfa_out") ;  */ 

/*strcpy(local->title, "Local  Z-Axes  in  Object  Frame,  zc  \n");*/ 
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/*m_out(zc,  1,  3,  1,  3,  local->title,  local->fname) ; */ 

/*  don’t  bother  printing  out  zc,  since  this  matrix  has  been  recycled  */ 

/*  print  out  variables  to  file  fname  */ 
strcpy(local->fname, "cf a_out") ; 

strcpy(local->title, "Position  Vector  to  Contacts,  r  \n"); 
m_out(local->r,  1,  3,  1,  3,  local->title,  local->fname) ; 

/  3|C  3|C  %  3|C  *  )|C  9|C  ]|C  1|C  :4c  ]}( Jte  3te  j|e  3|C  ate  3|e  %  :tc  3|C  iK  ifc  3|e:te  jfe  / 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(local->fname, "cf a.out") ;*/ 

/*strcpy(local->title, "Transformation  Matrix,  T_i  \n");*/ 

/*m_out(local->T_i,  1,  3,  1,  local->i3n,  local->title,  local->fname) ;*/ 

/^tctdloK*^***********:):*********:)!)^***’^:****/ 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(local->fname,"cfa_out") ;*/ 

/*strcpy(local->title, "Contact  Internal  Force  Vectors,  e_I  \n");*/ 
/*m_out(local->e_I,  1,  3,  1,  local->inn,  local->title,  local->fname) ;*/ 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(local->fname,"cfa_out") ;*/ 

/>i‘strcpy(local->title, "Internal  Force  Vectors  in  Contact  Frame,  e_Ic  \n");*/ 
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/*m_out(local->G_Ic,  1,  3,  1,  local->iim,  local->titlG,  local->fnainG) ;*/ 

/*  print  out  variablGS  to  filG  fnamG  */ 

/*strcpy(local->fnamG, "of a.out") ;*/ 

/*strcpy(local->titlG, "Grasp  Matrix,  W  \n");*/ 

/*m_out(local->W,  1,  6,  1,  local->i3n,  local->titlG,  local->fnamG) ;*/ 

/*  print  out  variablGs  to  fils  fnamG  */ 

/*strcpy(local->fnamG, "cfa_out") ;*/ 

/*strcpy(local->titlG,"PsuGdoInvGrsG  of  Grasp  Matrix,  Wp  \n");*/ 
/*m_out(local->Wp,  1,  local->i3n,  1,  6,  local->titlG,  local->fnamG) ;*/ 


return  I_0K; 

} 

/*  cfaCyclG  Process  module  information.  */ 

int  cfaCycle (local,  stask) 
cfaLocal.t  *local; 
sbsTask.t  *stask; 
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{ 

static  int  i,j; 

if  (local->s_flag[0]==l)  /*if  s_flag==l,  time  to  execute  h_sol,  */ 
/*if  s_flag==2,  ignore  this  cycle  */ 

{ 

kprintf ("About  to  start  the  h_sol  solution  function  \n"); 

local->time [0]=clock() ; 

/*  set  time  to  RTPU  clock  time  since  system  turned  on  (sec)  */ 

/*  for  a  given  object  wrench  Q,  calculate  the  contact  forces  */ 

h_sol(  local->mu[0] ,  local->del_X[0] ,  local->f c_x[0] ,  local->n[03, 
local->num_I ,  local->inn, 
local->RB_ef ,  local->c_dot ,  local->nc, 
local->cf d_wt ,  local->afd_wt , 
local->T_i,  local->e_I,  local->e_Ic, 
local->Fo,  local->F,  local->Q,  local->Wp, 
local->F_i_old,  local->Fc, 

local->theta,  local->f c.check,  local->ER_f,  local->ER_fc, 
local->vl_l,local->ER_dot,  local->wt_tot,  local->imf _ERd, 
local->mu_ERd,  local->wt_ef ,  local->nER_f ,  local->d_F, 
local->d_Fc,  local->d) ; 


/*  Reformat  force  vectors  is  matrix  format  as  noted  below  in  the  example  */ 
for(i=l ; i<=local->n[0] ;i++) 
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{ 

for(j=l; j<=3; j++) 

{ 

local->F_tempC(i-l)*local->nCO]+j]=local->F[j] [i] ; 

} 

} 

/*  set  time  equal  to  the  time  required  to  execute  h_sol  */ 
local->time[0]=clock()  -  local->time[0] ; 

/*  update  S_FLAG  to  2,  so  that  cfaload  will  execute  next  cycle  */ 
local->s_f lag [0] =2 ; 

kprintf ("Finished  the  h_sol  solution  function  \n"); 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(local->fname, "cfa_ wrench") ;*/ 

/*strcpy(local->title, "Input  Object  Wrench,  Q  \n");*/ 

/*v_out(local->q ,  1,  6,  local->title,  local->fname) ;*/ 

/  ****  !|:  ****  ****!)<***  IK  **  >|t  >•<  *******  >l<  I)!****  / 

/*****************)|!));*****i|ti|(i|t))<i^:)i:^i|c:|i****/ 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(local->fname, "cf a_out") ;*/ 

/*strcpy(local->title, "Contact  Forces,  Fo  \n");*/ 

/*m_out(local->Fo,  1,  3,  1,  local->n[0] ,  local->title,  local->fname) ;*/ 

/*  print  out  variables  to  file  fname  */ 
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/*strcpy(local->fname, "cf a.force") ;*/ 

/*strcpy(local->title, "Contact  Forces,  F  \n");*/ 

/*m_out(local->F,  1,  3,  1,  local->nC0]  ,  local->title,  local->fnaiiie)  ;*/ 

/  ))c))c  ^  i|c  ^  :|i  ’ll Ik  / 

/*  print  out  variables  to  file  fname  */ 
/*strcpy(local->fname,"cfa_time") ;*/ 

/*strcpy(local->title, "  \n") ;*/ 

/*s_out(local->time [0] ,  local->title,  local->fname) ; */ 

}  /*  end  if  s.flag  */ 


return  I_0K; 

} 

/*  cfaOff  Stop  the  module.  */ 

int  cfaOff (local,  stask) 
cfaLocal_t  ♦local; 
sbsTask.t  ♦stask; 

{ 

/♦release  memory  allocated  above  ♦/ 
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free_real_vector(local->R_temp, 1) ; 
free_real_vector(local->XC_teiap,  1)  ; 
f ree_real_vector (local->YC_temp , 1) ; 
f ree_real_vector (local->F_temp , 1) ; 
free_int .matrix (local->RB_ef ,1,7,1); 
f ree_r6al_vector(local->c_dot , 1) ; 
f ree_real_vector (local->cf d_wt , 1) ; 
f ree_real_vector (local->af d_wt , 1) ; 
f ree_real_matrix(local->r , 1 ,3 , 1) ; 
free_real_matrix(local->xc, 1 ,3, 1) ; 
free_rGal_matrix(local->yc, 1,3, 1) ; 
free_real_matrix(local->zc, 1 ,3, 1) ; 
f ree_real_matrix(local->Fo, 1 ,3 , 1) ; 
f ree_real_vector (local->Q , 1) ; 
f ree_real_matrix(local->F ,1,3,1); 
free_real_matrix(local->T_i, 1 ,3, 1) ; 
f ree_rGal_matrix(local->e_I ,1,3,1); 
free_real_matrix(local->e_Ic, 1,3,1) ; 
f ree_rGal_matrix(local->W ,1,6,1); 
f ree_real_matrix (local->Wp , 1 , local->i3n , 1) ; 

f ree_rGal_matrix(local->F_i_old, 1 ,3 , 1) ; 
f ree_real_matrix(local->Fc , 1,3,1); 
free_real_vector(local->theta,l) ; 
free_real_vector(local->f c_check,l) ; 
free_real_matrix(local->ER_f ,1 ,3,1) ; 
free_real_matrix(local->ER_fc, 1,3,1) ; 
free_real_vector(local->vl_l , 1)  ; 
free_real_vector (local->ER_dot , 1) ; 
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f ree_real_vector(local->wt_tot ,  1)  ,* 
f ree_real_vector (local->nER_f , 1) ; 
f ree.int .matrix (local->imf_ERd, 1 ,local->inn, 1) ; 
free_rGal_matrix(local->mu_ERd, l,local->inn,l) ; 
frGe_real_matrix(local->wt_Gf ,l,local->nc,  1) ; 
f rGG_rGal_matrix (local->d_F ,1,3,1); 
f rGG.rGal .matrix (local->d_Fc ,1,3,1); 
frGG.rGal.vGctor(local->d, 1) ; 

kprintf ("cf a:  OFF\n"); 

printf ("Stopping  thG  Contact  ForcG  Assignmont  Modulo  \n"); 
roturn  I. OK; 

} 

/*  cfaKill  Cloan  up  aftor  tho  modulo.  */ 

int  cfaKill (local,  stask) 
cfaLocal.t  *local; 
sbsTask.t  *stask; 

{ 

kprintf ("cfa:  FINISHED\n") ; 
return  I. OK; 

} 


/*  cfaError  Attempt  automatic  error  recovery.  */ 
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int  cfaError (local,  stask,  mptr,  ernnsg,  errcode) 

cfaLocal_t  *local; 

sbsTask.t  *stask; 

errModule.t  *mptr; 

char  *errmsg; 

int  errcode; 

{ 

/*  Return  after  not  correcting  error.  */ 

return  SBS_ERR0R; 

} 


/*  cfaClear  Clear  error  state  of  the  module.  */ 

int  cfaClear (local,  stask,  mptr,  errmsg,  errcode) 

cfaLocal.t  *local; 

sbsTask.t  *stask; 

errModule.t  *mptr; 

char  *errmsg; 

int  errcode; 

{ 

/*  Return  after  not  clearing  error.  */ 

sbsNewError (stask,  "Clear  not  defined,  still  in  error  state",  errcode); 
return  SBS_ERR0R; 
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} 


I  if  :4c  silicic  :|(a(c3|(afc9|(){(3|c]i(}|c3f(3|c%3tc}|c)|e)^s|(j4c^}|csje)tes|c:tc:|c:|c9|(:|()|«:te:|(^^){c)je9fc»|e»|c»|(9{e9{(9{e9f(  :4c  j 

/*  cfaSet  Set  module  parameters.  */ 

int  cfaSet (local,  stask) 
cfaLocal_t  *local; 
sbsTask_t  *stask; 

{ 

return  I_0K; 

} 

/*  cfaGet  Get  module  parameters.  */ 

/*  ***:(!=l‘***l)‘**************3(t********J|t************>(<**!|t***********>('>t'>t‘***>|t>('  */ 

int  cfaGet (local,  stask) 
cfaLocal.t  *local; 
sbsTask_t  *stask; 

{ 

return  I_0K; 

} 

/*  cfaSync  For  modules  which  synchronize  to  */ 

/*  something  other  than  the  clock.  */ 
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/if  ififififififififififififififififififiifififififififififififififififififififififififififififififififififififififififififififififififififif  */ 

int  cfaSync (local,  stask) 
cfaLocal_t  *local; 
sbsTask.t  ♦stask; 

{ 

return  I_0K; 

> 

void  h_sol(  float  mu,  float  del_X,  float  fc_x,  int  n,  int  num_I,  int  inn, 

int  **RB_ef,  float  c_dot[],  int  iniimmem, 

float  cf d_wt  □  ,  float  af d_wt [] , 

float  **T_i,  float  **e_I,  float  **e_Ic, 

float  **Fo,  float  **F,  float  Q[],  float  **Wp, 

float  **F_i_old,  float  **Fc, 

float  *theta,  float  *fc_check,  float  **ER_f,  float  **ER_fc, 
float  *vl,  float  *ER_dot,  float  *wt_tot,  int  ♦♦imf.ERd, 
float  **mu_ERd,  float  **wt_ef,  float  *nER_f,  float  **d_F, 
float  ♦*d_Fc,  float  *d) 

{ 

static  char  f name [80]; 
static  char  title [80]; 

static  int  i, j ,k,l, j j ,ii,imln, j jl,imlnl,imln2,imln3,I_count,i_wt_mf ,nr_flag; 
static  float  s\im,  suml,  ERdotmax,  fabsERdot,  C; 
static  float  a,b,c,dp,dn; 
static  float  mu2,  fc_x2,d_X,dX,dx; 
static  int  i_error; 
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/*  initialize  vectors  and  matrices  before  starting  the  solution.  */ 

inimat(l,3,l,n,F_i_old,0) ; 
inimat(l,3,l,n,Fc,0) ; 
inivecCl ,n, theta, 0) ; 
inivecCl ,n,f c_check,0) ; 
inimat(l,3,l,n,ER_f ,0) ; 
inimat(l,3,l,n,ER_fc,0) ; 
inivecCl, 3, vl,0) ; 
inivecCl, inn, ER_dot,0) ; 
inivecCl, inn, wt_tot,0) ; 
inivecCl, n,nER_f,0) ; 
ini_imatCl,inn,l,2,imf_ERd,0) ; 
inimat  C 1 , inn ,1,2 ,mu_ERd , 0) ; 
inimatCl,inummem,l,num_I,wt_ef ,0) ; 
inimatCl,3,l,n,d_F,0) ; 
inimatCl,3,l,n,d_Fc,0) ; 
inivecCl, inn, d,0) ; 


mu2=mu*mu ; 

f c_x2=f c_x*f c_x; 

/*  del.X  is  the  convergence  buffer,  fc_x  is  the  stability  offset  */ 
dX=del_X+fc_x; 


/*  calculate  external  contact  forces  based  on  psuedoinverse  and  object  wrench  */ 
forCi=l ; i<=n; i++) 
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for(l=l;l<=3;l++) 

■C 


sum=0; 

for(j=l; j<=6; j++) 

{ 

simi=s\am+Wp[(i-l)*3+l]  Cj]*QCj]  : 

} 

Fo[l]  [i]=suiii: 

} 


} 


/*  calculate  current  contact  forces  */ 
for(j=l; j<=n: j++) 


for(i=l;i<=3;i++) 

{ 

F[i]  Cj]=Fo[i]  [j]+F_i.oldCi]  [j]  ; 

} 


> 


/*  Fc=(T_i)’*F;  */ 

/*  convert  each  contact  force  into  local  contact  frame  coordinates  */ 
f or(i=l ; i<=n; i++) 

imln=(i-l)*n; 
for(j=l; j<=3; j++) 


jj=imln+j ; 


sum=0; 

for (k=l ; k<=3 ; k++) 

-C 

sum=sum+T_i  [k]  [jj]*F[k]  [i]  ; 

} 

FcCj]  [i]=suin; 

> 

/*  theta  is  eingle,  in  local  y-z  plane,  to  the  external  contact  point  */ 
thetaCi]=atan2(Fc[3]  [i]  ,Fc[2]  [i] ) ; 

} 

/*  check  to  see  if  any  current  contact  forces  */ 

/*  are  outside  of  the  friction  cones  */ 
f  or(i=l ;  i<=sn;  i++) 

{ 

fc_checkCi]  =  (-mu2*(Fc[l]  Ci]*FcCl]  [i]-2*Fc[l]  Ci]*fc_x)* 

(-(Fc[l] Ci]<0)+(Fc[l] [i] >=0) ) )-f c_x2*mu2+ 

(Fc  [2]  [i]  *Fc  [2]  [i]  +Fc  [3]  [i]  *Fc  [3]  [i]  ) ; 

> 

/*  begin  iterative  loop  to  find  viable  solution  */ 

i_error=0; 

while(mymax(l,n,fc_check)>0)  /*  contact  stability  violated  */ 

{ 

i_error=i_error+l ; 
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/*  print  out  variables  to  file  fname  */ 

/*strcpy(fname, "hs_out") ;*/ 

/*strcpy (title, "Contact  Force,  Contact  Frame,  Fc  \n");*/ 

/*m_out(Fc,  1,  3,  1,  n,  title,  fname);*/ 

/*  print  out  variables  to  file  fname  */ 

/♦strcpy (fname, "hs_out") ;*/ 

/*strcpy(title, "f c_check  \n") ;*/ 

/*v_out(f c_check,  1,  n,  title,  fname);*/ 

/*  determine  what  the  goal  vector  should  be,  */ 

/*  based  on  the  direction  of  Fo  */ 

for(i=l ; i<=n; i++) 

•C 

imln=(i-l)*n; 

/*  calculate  error  based  on  current  F  eind  goal  */ 

if (f c_checkCi] <=0)  /*contact  force  is  inside  friction  cone  */ 

{ 

for(j=l; j<=3; j++) 

{ 

ER_fc[j]  Ci]=0; 

> 

} 

/*  move  the  friction  cone  to  the  inside  of  the  nominal  cone  by  dX  */ 
/*  along  local  x-axis;  dX  is  used  for  solution  convergence,  not  */ 
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/*  contact  stability,  del_X  is  used  for  stability  */ 

else  /*  contact  force  is  outside  of  friction  cone  */ 

{ 

dx=(FcCl] [i]+mu*(FcC2] [i] *cos (theta [i] )+ 

Fc[3] [i] *sin(theta[i] )+mu*dX))/ (l+mu2) ; 

dx=dx*(dx>=dX)+dX*(dx<dX) ; 

ER.fcCl]  [i]=dx-Fc[l]  [i]  ; 

ER_fc[2] [i]  =  (dx-dX)*mu*cos(theta[i] )-Fc[2]  [i] ; 

ER_fc[3] [i]=(dx-dX)*mu*sin(thetaCi] )-Fc[3] [i] ; 

} 

/*  convert  errors  in  contact  force,  ER.fc,  into  object  frame  */ 
/*  ER_f=T_i*ER_fc;  */ 

for(k=l ;k<=3;k++) 

{ 

sum=0; 

for(j=l; j<=3; j++) 
jj=imln+j; 

sum=sum+T_i[k] [j j] *ER_f c [j] [i]  ; 

} 

ER_f Ck] Ci]=sum; 

} 


for(j=l; j<=3; j++) 
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vl[j]=ER_f  [j]  [i]  ; 

} 

nER_f Ci]=euclnorm(l,3,vl) ; 

} 

/  >|!  **  ****** >(C >|C )(C  J|< **  J|t **** >l<  *  111** ******** SK* "tt* / 

/*  print  out  variables  to  file  fname  */ 
/*strcpy(fname, "hs_out") ;*/ 

/*strcpy (title, "ER_f  \n");*/ 

/*m_out(ER_f ,  1,  3,  l,n,  title,  fname);*/ 
/************************************/ 

/*************************************/ 

/*  print  out  variables  to  file  fname  */ 
/*strcpy (fname , "hs_out " ) ; */ 

/♦strcpy (title, "nER.f  \n");*/ 

/*v_out(nER_f ,  1,  n,  title,  fname);*/ 
/************************************/ 

/*  loop  through  all  the  internal  forces  */ 
ERdotmax=0 ; 
for(i=l;i<=n;i++) 

{ 

imln=(i-l)*n; 


for(j=l; j<=n; j++) 

■c 

jj=imln+j ; 


/*  calculate  the  dot  product  between  the  contact  error  */ 
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/*  and  the  internal  force  for  each  contact  */ 


sum=0; 

for(k=l ;k<=3:k++) 

{ 

suiii=sum+e_Ic[k]  [j  j]*ER_fc[k]  [i]  ; 

} 

ER.dot  [j  j]=suin; 

/*  determine  max  value  of  ER_dot  for  normalization  or  ER_dot  */ 
f absERdot=f abs(ER_dot [j j] ) ; 
if(fabsERdot  >  ERdotmax) 

ERdotmax=f absERdot ; 


/*  can’t  divide  by  zero  in  normalization  */ 
if (ERdotmax==0) 

ERdotmax=l ; 

} 

f or (i=l ; i<=inn; i++) 

ER_dot [i] =ER_dot [i] /ERdotmax ; 

> 
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/*  print  out  variables  to  file  fname  */ 
/*strcpy(fnaine,"hs_out")  ;*/ 

/*strcpy (title, "ER_dot  \n")  ',*/ 
/*v_out(ER_dot,  l,inn,  title,  fname);*/ 

/ 1|!  *  !tc  i|c  )|(  >|c  >1: !)( iK  >l<  *******************  / 


/*  calculate  memberships  of  the  ER.dot  scalars  */ 
mem_edci(imf _ERd,  mu_ERd,  ER_dot,  inn,  c_dot,  inummem) ; 

/*************************************/ 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(fname,"hs_out") ;*/ 

/*strcpy (title, "imf.ERd  \n") ;*/ 

/*im_out(imf_ERd,  1, inn, 1,2,  title,  fname);*/ 

/itiiK**********************************/ 

/*************************************/ 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(fname,"hs_out") ;*/ 

/*strcpy (title, "mu_ERd  \n");*/ 

/*m_out(mu_ERd,  1, inn, 1,2,  title,  fname);*/ 

***************************  ********/ 

/*  now  apply  the  rule  base  to  the  input  data  in  order  to  weight  the  */ 

/*  internal  forces  */ 

inimat (1, inummem, l,num_ I, wt_ef,0) ; 

l_count=0; 

for(i=l;i<=n;i++) 

{ 
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for(j=l; j<=n; j++) 

{ 

if(i!=j  &a  i<j) 

{ 

jj=(i-l)*n+j ; 
ii=(j-l)*n+i; 

I_count=I_count+l ; 

for(l=l;l<=2;l++) 

{ 

for(k=l ;k<=2;k++) 

{ 

vlCl]=mu_ERd[j  j]  [1]  ; 
vl [2] =mu_ERd [ii] [k] ; 

i_wt_mf=RB_ef [imf _ERd[j j] [1]] [imf_ERd[ii]  [k]]  ; 
wt.ef [i_wt_mf] [I.count] =wt_ef [i.wt.mf] [I_coimt]+mymin(l,2,vl) ; 

} 

} 

> 

> 

} 

i 

/*  at  this  point,  ’I_count’  should  equal  ’num_I’*/ 

/*  which  is  the  number  of  internal  forces  */ 

f or(i=l ; i<=num_I ; i++) 

{ 

sum=0; 

suml=0: 


C-54 


for(j=l ; j<=inummem: j++) 

{ 

suin=suni+af d_wt  [j]  *cf d_wt  [j]  *wt_ef  [j]  [i]  ; 
suinl=suml+afd_wt  [j]  *wt_ef  [j]  Ci]  ; 

> 

wt.tot [i] =sum/suml ; 

} 

/*  print  out  variables  to  file  fname  */ 
/♦strcpy (fname , "hs_out") ; */ 
/*strcpy(title,"wt_tot  \n");*/ 

/*v_out(wt_tot,  1,  num_I,  title,  fname);*/ 

inimat(l,3,l,n,d_F,0) ; 

I _ count =0; 
for(i=l;i<=n;i++) 

{ 

for(j=l; j<=n; j++) 

-C 

if(i!=j  &£  i<j) 

{ 

j j=(i-l)*n+j ; 
ii=(j-l)*n+i; 

I_count=I_coiant+l ; 

for(k=l ;k<=3;k++) 

{ 

d_F[k]  Ci]=d_FCk]  [i]+wt_tot[I_count]*e_I[k]  [jj]  ; 
d_F[k]  Cj]=d_F[k]  Cj]+wt_tot[I_co;mt]*e_I[k]  [ii]  ; 
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> 

} 

} 

} 

/*  print  out  variables  to  file  fname  */ 
/*strcpy(fnaine,"hs_out")  ;*/ 

/*strcpy (title, "d_F  \n");*/ 

/*m_out(d_F,  1,  3,  1,  n,  title,  fname);*/ 

/>|c!tc**************>f:*>tc*****************/ 

/*  determine  contact,  j,  with  most  significant  error,  sum  */ 
s;nn=0; 

for(i=l ; i<=n; i++) 

{ 

if (nER.f [i] >sum) 

{ 

3=i; 

sum=nER_f [i]  ; 

} 

> 

/*  determine  the  psuedoinverse  of  j’th  column  of  d_F  */ 

/*  since  vl  is  a  vector,  the  transpose  of  the  psuedoinverse  is:  */ 
/*  vl#t=vl  /  (vl[l]~2  +  vl[2]"2  +  vlC3]~2)  */ 

siim=0; 

for(i=l;i<=3;i++) 

■C 
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sum=sum+d_F[i]  [j3*ci_F[i]  [j]  ; 

> 

for(i=l;i<=3;i++) 

{ 

vl[i]=d_FCi]  Cj]/sum; 

} 


/*  vl  is  now  the  transpose  of  the  psuedoinverse  of  d_F[:][j]  */ 

C=0; 

for(i=l ; i<=3; i++) 

-C 

C=C+vl [i] *ER_f Ci] Cj] ; 

} 

/*  C  is  the  scale  factor  for  the  change  in  contact  forces,  d_F  */ 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(fname, "hs_out") ;*/ 

/*strcpy(title,"C  \n"):*/ 

/*s_out(C,  title,  fname);*/ 


for(i=l ; i<=n; i++) 

•C 

for(j=l; j<=3; j++) 

{ 

F_i_oldCj]  [i]=F_i_oldCj]  [i]+C*d_F[j]  [i]  ; 

> 
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> 

/*  update  contact  forces  */ 
for(j=l; j<=n; j++) 

{ 

for(i=l;i<=3;i++) 

FCi]  Cj]=FoCi]  [j3+F_i_oldCi]  [j]  ; 

> 

> 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(fname, "hs_out") ;*/ 

/♦strcpy (title, "F  \n");*/ 

/*m_out(F,  l,3,l,n,  title,  fname);*/ 

/*  Fc=(T_i)’*F;  */ 

/*  convert  each  contact  force  into  local  contact  frame  coordinates  */ 
for(i=l ; i<=n; i++) 

{ 

imln=(i-l)*n; 
for(j=l; j<=3; j++) 

{ 

jj=imln+j : 
sum=0; 
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for(k=l;k<=3;k++) 

{ 


sum=sum+T_i[k]  [jj]*FCk]  [i]  ; 


} 

Fc[j]  [i]=sum; 

> 

} 


/*  check  to  see  if  any  current  contact  forces  */ 

/*  are  outside  of  the  friction  cones  */ 
for(i=l;i<=n;i++) 

if (i_error<15) 

{ 

fc_checkCi]  =  (-mu2*(FcCl]  [i]*Fc[l]  Ci]-2*Fc[l]  [i3*fc_x)* 

(-(Fc[l] Ci]<0)+(FcCl] [i]>=0)))-fc_x2*mu2+ 

(Fc  [2]  [i]  *Fc  [2]  Ci]  +Fc  [3]  [i]  *Fc  [3]  [i]  )  ; 

> 

else 

fc_checkCi]=-l; 

/*  print  out  variables  to  file  fname  */ 

/*strcpy(fname,"hs_out") ;*/ 

/*strcpy(title, "i.error,  Over  15  Attempts  at  Solution  Failed  \n");*/ 
/*s_out(i_error,  title,  fname);*/ 

/  9|ci|ci|c>|c)|c)|(  **  l)c :)( l|c  itc  l|c  i|ci|c  It:  i|(  )|c  ))ciK  ***********  ****  / 

} 

} 


C-59 


}  /*  Gnd  while  loop  for  contact  stability  */ 

}  /*  end  h_sol  */ 

float  mymaxCint  1,  int  u,  float  b[]) 

/*  this  function  takes  a  vector  and  returns  the  maximum  value  of  the  vector  */ 
/*  1  and  u  are  the  lower  and  upper  indices  of  the  vector.  */ 

int  i; 
float  max; 

max=b  [1] ; 

f or (i=l+l ; i<=u; i++) 

{ 

if  (b  [i]  >max) 

{ 

max=b [i] ; 

} 

> 

return  max; 
y  /*  end  max  */ 


float  myminCint  1,  int  u,  float  bG) 

/*  this  function  takes  a  vector  and  returns  the  minimum  value  of  the  vector  */ 
/*  1  and  u  are  the  lower  and  upper  indices  of  the  vector.  */ 
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int  i; 
float  min; 

min=b [1] ; 


f or(i=l+l ; i<=u: i++) 

if  (b[i]  <min) 

min=b  [i]  ; 

} 

} 

return  min; 

}  /*  end  min  */ 

void  mem_edci(  int  **imf_ERd,  float  **mu_ERd,  float  ER.dotC], 
int  n,  float  c_dot[],  int  m) 

{ 

/*  subroutine  which  calculates  the  membership  of  a  given  input  vector  */ 

/*  with  respect  to  the  triangular  fuzzy  membership  functions  defined  */ 

/*  by  the  vector  of  ’centers.’  Use  this  version  of  membership  calc-  */ 

/*  ulation  for  external  force,  dot  product  of  internal  and  external  forces,*/ 
/*  cross  product  of  internal  force  and  friction  cone  vectors,  and  output  */ 

/*  weights  associated  with  each  internal  force.  */ 

/*  Mark  Hunter,  20  Nov  95,  Air  Force  Institute  of  Technology,  WPAFB,  OH.  */ 

/*  mf  is  the  membership  function  label  of  the  first  non-zero  membership  */ 

/*  mu(i,l)  is  the  value  of  the  membership  of  x(i)  with  repect  to  mf(i)  */ 

/*  index(i)  refers  to  the  number  of  membership  functions  crossed  by  x(i)  */ 

/*  n  is  the  number  of  contacts  squared  */ 
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/*  m  is  the  number  of  membership  functions  across  the  domain  */ 


int  i,  j,  j.flag; 


for(i=l ; i<=n; i++) 

■C 

j=0; 

j_flag=0; 

if (ER.dot [i] <=c_dot [1] ) 

{ 

j_flag=l; 

imf_ERd[i]  [1]=1: 
imf.ERdCi] [2]  =2; 
mu_ERd[i]  Cl]=l: 
mu.ERdCi]  C2]=0; 

> 

else  if (ER_dot [i] >=c_dot Cm] ) 

{ 

j_flag=l; 

imf_ERd[i] [l]=m-l; 
imf.ERdCi] C2]=m; 
mu.ERdCi]  Cl]=0; 
mu.ERdCi]  C2]  =  l; 

> 

while (j  _f lag==0) 
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j=j  +  l; 


if  (ER_dot[i]<c_dot  Cj+1] ) 

{ 

j_flag=l; 

imf_ERd[i]  Cl]=j ; 
imf_ERd[i] [2]=j+l; 

mu_ERd[i]  [l]  =  (c_dotCj+l]  -  ER_dot[i])/ 
(c_dot[j+l]  -  c_dot[j]); 
mu_ERd[i]  [2]  =  (ER_dot[i]  -  c_dot[j])/ 
(c_dot[j+l]  -  c_dot[j]); 

> 


}  /*  end  while  */ 

}  /*  end  for  */ 

}  /*  end  mem.edci  */ 

void  cross(int  1,  int  u,  float  vl[],  float  v2[],  float  vSC]) 

■C 

int  m=u-l; 

v3  [1]  =vl  Cm]  *v2  [u]  -v2 [m]  *vl  [u]  ; 
v3  Cm]  =vl  Cu]  *v2  Cl]  -v2  Cu]  *vl  Cl]  ; 
v3  Cu]  =vl  Cl]  *v2  Cm]  -v2 Cl]  *vl  Cm]  ; 

> 
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float  GuclnormCint  1,  int  u,  float  b[]) 

/*  calculate  euclidean  norm  of  vector  a[l:u]  */ 
{- 

int  i; 

float  enorm=0.0; 

for  (i=l;  i<=u;  i++) 

{ 

enorm  =  enorm  +  b  [i] *b  [i] ; 

} 

return  sqrt( enorm); 

} 

void  m_ out (float  **a,  int  rl,  int  ru,  int  cl, 
int  cu,  char  titled,  char  filed) 

/*  print  matrix  value  to  file  */ 

i 

char  fn[8]  ; 
int  i , j ; 

FILE  *fp; 

for  (i=0;  i<8;  i++) 

-C 

fnCi]=f  ile[i]  ; 

} 

fp  =  fopen  (fn,"a"); 
fprintf  (fp,"y,s"  , title) ; 
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for  (i=rl;i<=ru;i++) 

{ 

fprintf (fp, "\n") ; 
for  (j=cl; j<=cu; j++) 

fprintfCfp,"  y.l2.4e  ",  a[i][j]); 

} 

> 

fprintf (f p , " \n" ) : 
f close(fp) : 

> 

void  im_out(int  **a,  int  rl,  int  ru, 
int  cl,  int  cu,  char  titleC],  char  file[]) 
/*  print  matrix  value  to  file  */ 

■C 

char  fn[8] ; 
int  i,j; 

FILE  *fp; 

for  (i=0;  i<8;  i++) 

{ 

fnCi]=file[i]  ; 

> 


fp  =  fopen  (fn,"a"); 
fprintf  (fp,  "'/,s" , title) ; 
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for  (i=rl;i<=ru;i++) 

{ 

fprintf (fp, "\n") ; 
for  (j=cl; j<=cu; j++) 

fprintfCfp,"  '/,d  ",  aCi][j]); 

} 

> 

fprintf (fp, "\n") ; 
f close(fp) ; 

} 

void  v.out (float  b[],  int  1,  int  u, 
char  titleC],  char  fileG) 

/*  print  vector  value  to  file  */ 

i 

char  fnC8]  ; 
int  i; 

FILE  *fp; 

for  (i=0;  i<8;  i++) 

{ 

fn[i]=file[i]  ; 

> 


fp  =  fopen  (fn,"a"); 
fprintf  (fp,  '"/.s"  , title) ; 
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for  (i=l; i<=u; i++) 

{ 

fprintf(fp,"  */,12.4e  \n  ",b[i]); 

} 

fprintf (fp, "\n") ; 
f close(fp) : 

> 

void  s_out (float  c,  char  titleC],  char  file[]) 
/*  print  scalar  value  to  file  */ 

char  fnC8]  ; 
int  i; 

FILE  *fp; 

for  (i=0;  i<8:  i++) 

{ 

fnCi]=f  ile[i]  ; 

} 

fp  =  fopen  (fn,"a"): 
fprintf  (fp,  "'/,s"  .title) ; 
fprintf  (fp,"  */,12.4e  \n  ",c); 
fprintf (fp,"\n") ; 
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f close(fp) ; 

} 
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